diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index d101c0c52c..a5441d52cb 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -28,7 +28,8 @@ def add_arguments(self, parser, cli_name): # noqa: D102 ) parser.add_argument( '-v', '--verbose', action='store_true', - help='Display request/response information for services' + help='Display verbose information about request/response services, actions and' + ' print messages size contribution on per topic bases.' ) available_sorting_methods = Info().get_sorting_methods() parser.add_argument( diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index dadb6807ba..195f3ceaf8 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -70,27 +70,34 @@ def add_recorder_arguments(parser: ArgumentParser) -> None: parser.add_argument( '--services', type=str, metavar='ServiceName', nargs='+', help='Space-delimited list of services to record.') + parser.add_argument( + '--actions', type=str, metavar='ActionName', nargs='+', + help='Space-delimited list of actions to record.') parser.add_argument( '--topic-types', nargs='+', default=[], metavar='TopicType', help='Space-delimited list of topic types to record.') parser.add_argument( '-a', '--all', action='store_true', - help='Record all topics and services (Exclude hidden topic).') + help='Record all topics, services and actions (Exclude hidden topic).') parser.add_argument( '--all-topics', action='store_true', help='Record all topics (Exclude hidden topic).') parser.add_argument( '--all-services', action='store_true', help='Record all services via service event topics.') + parser.add_argument( + '--all-actions', action='store_true', + help='Record all actions via internal topics and service event topics.') parser.add_argument( '-e', '--regex', default='', help='Record only topics and services containing provided regular expression. ' - 'Note: --all, --all-topics or --all-services will override --regex.') + 'Note: --all, --all-topics, --all-services or --all-actions will override --regex.') parser.add_argument( '--exclude-regex', default='', help='Exclude topics and services containing provided regular expression. ' 'Works on top of ' - '--all, --all-topics, --all-services, --topics, --services or --regex.') + '--all, --all-topics, --all-services, --all-actions, --topics, --services, --actions ' + 'or --regex.') parser.add_argument( '--exclude-topic-types', type=str, default=[], metavar='ExcludeTopicTypes', nargs='+', help='Space-delimited list of topic types not being recorded. ' @@ -103,6 +110,10 @@ def add_recorder_arguments(parser: ArgumentParser) -> None: '--exclude-services', type=str, metavar='ServiceName', nargs='+', help='Space-delimited list of services not being recorded. ' 'Works on top of --all, --all-services, --services or --regex.') + parser.add_argument( + '--exclude-actions', type=str, metavar='ActionName', nargs='+', + help='Space-delimited list of actions not being recorded. ' + 'Works on top of --all, --all-actions, --actions or --regex.') # Discovery behavior parser.add_argument( @@ -217,10 +228,11 @@ def add_recorder_arguments(parser: ArgumentParser) -> None: def check_necessary_argument(args): - # At least one options out of --all, --all-topics, --all-services, --services, --topics, - # --topic-types or --regex must be used - if not (args.all or args.all_topics or args.all_services or + # At least one options out of --all, --all-topics, --all-services, --all-actions, --services, + # --actions --topics, --topic-types or --regex must be used + if not (args.all or args.all_topics or args.all_services or args.all_actions or (args.services and len(args.services) > 0) or + (args.actions and len(args.actions) > 0) or (args.topics and len(args.topics) > 0) or (args.topic_types and len(args.topic_types) > 0) or args.regex): return False @@ -239,9 +251,9 @@ def validate_parsed_arguments(args, uri) -> str: if args.exclude_regex and not \ (args.all or args.all_topics or args.topic_types or args.all_services or - args.regex): + args.all_actions or args.regex): return print_error('--exclude-regex argument requires either --all, ' - '--all-topics, --topic-types, --all-services or --regex') + '--all-topics, --topic-types, --all-services, --all-actions or --regex') if args.exclude_topics and not \ (args.all or args.all_topics or args.topic_types or args.regex): @@ -257,14 +269,22 @@ def validate_parsed_arguments(args, uri) -> str: return print_error('--exclude-services argument requires either --all, --all-services ' 'or --regex') + if args.exclude_actions and not (args.all or args.all_actions or args.regex): + return print_error('--exclude-actions argument requires either --all, --all-actions ' + 'or --regex') + if (args.all or args.all_services) and args.services: print(print_warn('--all or --all-services will override --services')) if (args.all or args.all_topics) and args.topics: print(print_warn('--all or --all-topics will override --topics')) - if (args.all or args.all_topics or args.all_services) and args.regex: - print(print_warn('--all, --all-topics or --all-services will override --regex')) + if (args.all or args.all_actions) and args.actions: + print(print_warn('--all or --all-actions will override --actions')) + + if (args.all or args.all_topics or args.all_services or args.all_actions) and args.regex: + print(print_warn('--all, --all-topics --all-services or --all-actions will override ' + '--regex')) if os.path.isdir(uri): return print_error("Output folder '{}' already exists.".format(uri)) @@ -281,6 +301,8 @@ def validate_parsed_arguments(args, uri) -> str: if args.compression_queue_size < 0: return print_error('Compression queue size must be at least 0.') + return None + class RecordVerb(VerbExtension): """Record ROS data to a bag.""" @@ -330,11 +352,14 @@ def main(self, *, args): # noqa: D102 record_options = RecordOptions() record_options.all_topics = args.all_topics or args.all record_options.all_services = args.all_services or args.all + record_options.all_actions = args.all_actions or args.all record_options.is_discovery_disabled = args.no_discovery record_options.topics = args.topics record_options.topic_types = args.topic_types # Convert service name to service event topic name record_options.services = convert_service_to_service_event_topic(args.services) + record_options.actions = args.actions if args.actions else [] + record_options.exclude_topic_types = args.exclude_topic_types record_options.rmw_serialization_format = args.serialization_format record_options.topic_polling_interval = datetime.timedelta( @@ -344,6 +369,7 @@ def main(self, *, args): # noqa: D102 record_options.exclude_topics = args.exclude_topics if args.exclude_topics else [] record_options.exclude_service_events = \ convert_service_to_service_event_topic(args.exclude_services) + record_options.exclude_actions = args.exclude_actions if args.exclude_actions else [] record_options.node_prefix = NODE_NAME_PREFIX record_options.compression_mode = args.compression_mode record_options.compression_format = args.compression_format diff --git a/ros2bag/test/test_recorder_args_parser.py b/ros2bag/test/test_recorder_args_parser.py index 7d8c2def68..1ebfba36c4 100644 --- a/ros2bag/test/test_recorder_args_parser.py +++ b/ros2bag/test/test_recorder_args_parser.py @@ -63,6 +63,16 @@ def test_recorder_services_list_argument(test_arguments_parser): assert output_path.as_posix() == args.output +def test_recorder_actions_list_argument(test_arguments_parser): + """Test recorder --actions list argument parser.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--actions', 'action1', 'action2', '--output', output_path.as_posix()] + ) + assert ['action1', 'action2'] == args.actions + assert output_path.as_posix() == args.output + + def test_recorder_services_and_positional_topics_list_arguments(test_arguments_parser): """Test recorder --services list and positional topics list arguments parser.""" output_path = RESOURCES_PATH / 'ros2bag_tmp_file' @@ -74,6 +84,17 @@ def test_recorder_services_and_positional_topics_list_arguments(test_arguments_p assert output_path.as_posix() == args.output +def test_recorder_actions_and_positional_topics_list_arguments(test_arguments_parser): + """Test recorder --actions list and positional topics list arguments parser.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--output', output_path.as_posix(), + '--actions', 'action1', 'action2', '--', 'topic1', 'topic2']) + assert ['action1', 'action2'] == args.actions + assert ['topic1', 'topic2'] == args.topics_positional + assert output_path.as_posix() == args.output + + def test_recorder_services_and_optional_topics_list_arguments(test_arguments_parser): """Test recorder --services list and optional --topics list arguments parser.""" output_path = RESOURCES_PATH / 'ros2bag_tmp_file' @@ -85,6 +106,41 @@ def test_recorder_services_and_optional_topics_list_arguments(test_arguments_par assert output_path.as_posix() == args.output +def test_recorder_actions_and_optional_topics_list_arguments(test_arguments_parser): + """Test recorder --actions list and optional --topics list arguments parser.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--output', output_path.as_posix(), + '--actions', 'action1', 'action2', '--topics', 'topic1', 'topic2']) + assert ['action1', 'action2'] == args.actions + assert ['topic1', 'topic2'] == args.topics + assert output_path.as_posix() == args.output + + +def test_recorder_services_and_actions_list_arguments(test_arguments_parser): + """Test recorder --services list and --actions list arguments parser.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--output', output_path.as_posix(), + '--services', 'service1', 'service2', '--actions', 'action1', 'action2']) + assert ['service1', 'service2'] == args.services + assert ['action1', 'action2'] == args.actions + assert output_path.as_posix() == args.output + + +def test_recorder_services_actions_and_topics_list_arguments(test_arguments_parser): + """Test recorder --services list, --actions list and --topics list arguments parser.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--output', output_path.as_posix(), + '--services', 'service1', 'service2', '--actions', 'action1', 'action2', + '--topics', 'topic1', 'topic2']) + assert ['service1', 'service2'] == args.services + assert ['action1', 'action2'] == args.actions + assert ['topic1', 'topic2'] == args.topics + assert output_path.as_posix() == args.output + + def test_recorder_topic_types_list_argument(test_arguments_parser): """Test recorder --topic-types list argument parser.""" output_path = RESOURCES_PATH / 'ros2bag_tmp_file' @@ -124,6 +180,16 @@ def test_recorder_exclude_services_list_argument(test_arguments_parser): assert output_path.as_posix() == args.output +def test_recorder_exclude_actions_list_argument(test_arguments_parser): + """Test recorder --exclude-actions list argument parser.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--exclude-actions', 'action1', 'action2', '--output', output_path.as_posix()] + ) + assert ['action1', 'action2'] == args.exclude_actions + assert output_path.as_posix() == args.output + + def test_recorder_custom_data_list_argument(test_arguments_parser): """Test recorder --custom-data list argument parser.""" output_path = RESOURCES_PATH / 'ros2bag_tmp_file' @@ -138,21 +204,22 @@ def test_recorder_validate_exclude_regex_needs_inclusive_args(test_arguments_par """Test that --exclude-regex needs inclusive arguments.""" output_path = RESOURCES_PATH / 'ros2bag_tmp_file' args = test_arguments_parser.parse_args( - ['--exclude-regex', '%s-%s', '--services', 'service1', '--topics', 'topic1', - '--output', output_path.as_posix()] + ['--exclude-regex', '%s-%s', '--services', 'service1', '--topics', 'topic1', '--actions', + 'action1', '--output', output_path.as_posix()] ) assert '%s-%s' == args.exclude_regex assert args.all is False assert args.all_topics is False assert [] == args.topic_types assert args.all_services is False + assert args.all_actions is False assert '' == args.regex uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S') error_str = validate_parsed_arguments(args, uri) assert error_str is not None expected_output = '--exclude-regex argument requires either --all, ' \ - '--all-topics, --topic-types, --all-services or --regex' + '--all-topics, --topic-types, --all-services, --all-actions or --regex' matches = expected_output in error_str assert matches, ERROR_STRING_MSG.format(expected_output, error_str) @@ -227,3 +294,28 @@ def test_recorder_validate_exclude_services_needs_inclusive_args(test_arguments_ 'or --regex' matches = expected_output in error_str assert matches, ERROR_STRING_MSG.format(expected_output, error_str) + + +def test_recorder_validate_exclude_actions_needs_inclusive_args(test_arguments_parser): + """Test that --exclude-actions needs at least --all, --all-actions or --regex arguments.""" + output_path = RESOURCES_PATH / 'ros2bag_tmp_file' + args = test_arguments_parser.parse_args( + ['--exclude-actions', 'action1', '--actions', 'action1', '--all-topics', + '--output', output_path.as_posix()] + ) + assert ['action1'] == args.exclude_actions + assert args.all is False + assert args.all_topics is True + assert [] == args.topic_types + assert args.all_services is False + assert args.all_actions is False + assert ['action1'] == args.actions + assert '' == args.regex + assert '' == args.exclude_regex + + uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S') + error_str = validate_parsed_arguments(args, uri) + assert error_str is not None + expected_output = '--exclude-actions argument requires either --all, --all-actions or --regex' + matches = expected_output in error_str + assert matches, ERROR_STRING_MSG.format(expected_output, error_str) diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index 0eb646bdac..df76e5c2be 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -73,7 +73,8 @@ add_library(${PROJECT_NAME} SHARED src/rosbag2_cpp/writer.cpp src/rosbag2_cpp/writers/sequential_writer.cpp src/rosbag2_cpp/reindexer.cpp - src/rosbag2_cpp/service_utils.cpp) + src/rosbag2_cpp/service_utils.cpp + src/rosbag2_cpp/action_utils.cpp) target_link_libraries(${PROJECT_NAME} PUBLIC pluginlib::pluginlib @@ -312,6 +313,16 @@ if(BUILD_TESTING) ${test_msgs_TARGETS} ) endif() + + ament_add_gmock(test_action_utils + test/rosbag2_cpp/test_action_utils.cpp) + if(TARGET test_action_utils) + target_link_libraries(test_action_utils + ${PROJECT_NAME} + rosbag2_test_common::rosbag2_test_common + ${test_msgs_TARGETS} + ) + endif() endif() ament_package() diff --git a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp new file mode 100644 index 0000000000..950768197c --- /dev/null +++ b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp @@ -0,0 +1,91 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_CPP__ACTION_UTILS_HPP_ +#define ROSBAG2_CPP__ACTION_UTILS_HPP_ + +#include +#include + +#include "rosbag2_cpp/visibility_control.hpp" + +namespace rosbag2_cpp +{ +/// \brief Enum class for action introspection interface types. +enum class ActionInterfaceType +{ + SendGoalEvent, + CancelGoalEvent, + GetResultEvent, + Feedback, + Status, + Unknown +}; + +/// \brief Check if the topic belong to the action introspection interface. +/// \param topic_name Topic name. +/// \param topic_type Topic type. +/// \return Boolean value indicating if the topic belongs to the action introspection interface. +ROSBAG2_CPP_PUBLIC +bool +is_topic_belong_to_action(const std::string & topic_name, const std::string & topic_type); + +/// \brief Check if the topic belong to the action introspection interface. +/// \param action_interface_type Action interface type given from the +/// get_action_interface_type(const std::string & topic_name) function call. +/// \param topic_type +/// \return Boolean value indicating if the topic belongs to the action introspection interface. +ROSBAG2_CPP_PUBLIC +bool +is_topic_belong_to_action( + ActionInterfaceType action_interface_type, const std::string & topic_type); + +/// \brief Transform the action's introspection topic name to the short action name. +/// \param topic_name Topic name. +/// \note Call this function after is_topic_belong_to_action() returns true +/// \return String with short action name. +/// \example if topic_name is "/fibonacci/_action/send_goal/_service_event" the function will +/// return the short action name "/fibonacci" +ROSBAG2_CPP_PUBLIC +std::string +action_interface_name_to_action_name(const std::string & topic_name); + +/// \brief Extract the original action type from the action's introspection type. +/// \note Call this function after is_topic_belong_to_action() returns true +/// \param topic_type Topic type name. +/// \return Original action type name aka "example_interfaces/action/Fibonacci" if action interface +//// type is SendGoalEvent, GetResultEvent or Feedback message, if action interface type is +/// CancelGoalEvent or Status, return empty string. +/// \throw std::runtime_error for unknown action interface type +ROSBAG2_CPP_PUBLIC +std::string +get_action_type_for_info(const std::string & topic_type); + +/// \brief Extract the action interface type from the action's introspection topic name. +/// \param topic_name Topic name. +/// \return ActionInterfaceType enum value. +ROSBAG2_CPP_PUBLIC +ActionInterfaceType +get_action_interface_type(const std::string & topic_name); + +/// \brief Transform the action name returned from the `action_interface_name_to_action_name(..) +/// to the list of the action introspection topics. +/// \param action_name Action name returned from the `action_interface_name_to_action_name(..)`. +/// \return Vector of action introspection topics corresponding to the action name. +ROSBAG2_CPP_PUBLIC +std::vector +action_name_to_action_interface_names(const std::string & action_name); +} // namespace rosbag2_cpp + +#endif // ROSBAG2_CPP__ACTION_UTILS_HPP_ diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index bed62d4845..d0d4190234 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rosbag2_cpp/visibility_control.hpp" @@ -36,6 +37,20 @@ typedef ROSBAG2_CPP_PUBLIC_TYPE struct rosbag2_service_info_t size_t response_count; } rosbag2_service_info_t; +typedef ROSBAG2_CPP_PUBLIC_TYPE struct rosbag2_action_info_t +{ + std::string name; + std::string type; + std::string serialization_format; + // request_count and response_count + std::pair send_goal_service_msg_count; + std::pair cancel_goal_service_msg_count; + std::pair get_result_service_msg_count; + // message count + size_t feedback_topic_msg_count; + size_t status_topic_msg_count; +} rosbag2_action_info_t; + class ROSBAG2_CPP_PUBLIC Info { public: @@ -44,8 +59,11 @@ class ROSBAG2_CPP_PUBLIC Info virtual rosbag2_storage::BagMetadata read_metadata( const std::string & uri, const std::string & storage_id = ""); - virtual std::vector> read_service_info( - const std::string & uri, const std::string & storage_id = ""); + std::pair< + std::vector>, + std::vector> + > + read_service_and_action_info(const std::string & uri, const std::string & storage_id = ""); virtual std::unordered_map compute_messages_size_contribution( const std::string & uri, const std::string & storage_id = ""); diff --git a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp new file mode 100644 index 0000000000..ece759a5cb --- /dev/null +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -0,0 +1,149 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rosbag2_cpp/action_utils.hpp" + +namespace rosbag2_cpp +{ +// The postfix of the action internal topics and service event topics +const std::unordered_map action_topic_to_postfix_map = { + {ActionInterfaceType::SendGoalEvent, "/_action/send_goal/_service_event"}, + {ActionInterfaceType::CancelGoalEvent, "/_action/cancel_goal/_service_event"}, + {ActionInterfaceType::GetResultEvent, "/_action/get_result/_service_event"}, + {ActionInterfaceType::Feedback, "/_action/feedback"}, + {ActionInterfaceType::Status, "/_action/status"} +}; + +// The regex pattern of the action internal topics and service event topics +const std::unordered_map action_topic_type_to_regex_map = { + {ActionInterfaceType::SendGoalEvent, ".+/action/.+SendGoal_Event$"}, + {ActionInterfaceType::CancelGoalEvent, "^action_msgs/srv/CancelGoal_Event$"}, + {ActionInterfaceType::GetResultEvent, ".+/action/.+GetResult_Event$"}, + {ActionInterfaceType::Feedback, ".+/action/.+_FeedbackMessage$"}, + {ActionInterfaceType::Status, "^action_msgs/msg/GoalStatusArray$"} +}; + +const size_t kMinActionTopicPostfixLen = + action_topic_to_postfix_map.at(ActionInterfaceType::Status).length(); + +bool is_topic_belong_to_action(const std::string & topic_name, const std::string & topic_type) +{ + ActionInterfaceType topic = get_action_interface_type(topic_name); + if (topic == ActionInterfaceType::Unknown) { + return false; + } + + std::regex pattern(action_topic_type_to_regex_map.at(topic)); + return std::regex_search(topic_type, pattern); +} + +bool is_topic_belong_to_action( + ActionInterfaceType action_interface_type, const std::string & topic_type) +{ + if (action_interface_type == ActionInterfaceType::Unknown) { + return false; + } + std::regex pattern(action_topic_type_to_regex_map.at(action_interface_type)); + return std::regex_search(topic_type, pattern); +} + +std::string action_interface_name_to_action_name(const std::string & topic_name) +{ + std::string action_name; + if (topic_name.length() <= kMinActionTopicPostfixLen) { + return action_name; + } else { + for (const auto & [topic_type_enum, postfix] : action_topic_to_postfix_map) { + if (topic_name.length() > postfix.length() && + topic_name.compare(topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) + { + action_name = topic_name.substr(0, topic_name.length() - postfix.length()); + break; + } + } + } + + return action_name; +} + +std::string get_action_type_for_info(const std::string & topic_type) +{ + std::string action_type; + + for (auto &[topic_type_enum, regex] : action_topic_type_to_regex_map) { + std::regex pattern(regex); + if (std::regex_search(topic_type, pattern)) { + switch (topic_type_enum) { + case ActionInterfaceType::SendGoalEvent: + // Remove the postfix "_SendGoal_Event" + action_type = + topic_type.substr(0, topic_type.length() - std::strlen("_SendGoal_Event")); + break; + case ActionInterfaceType::GetResultEvent: + // Remove the postfix "_GetResult_Event" + action_type = + topic_type.substr(0, topic_type.length() - std::strlen("_GetResult_Event")); + break; + case ActionInterfaceType::Feedback: + // Remove the postfix "_FeedbackMessage" + action_type = + topic_type.substr(0, topic_type.length() - std::strlen("_FeedbackMessage")); + break; + case ActionInterfaceType::CancelGoalEvent: + case ActionInterfaceType::Status: + break; + default: + throw std::runtime_error("Can't get action type. Unknown action interface type"); + break; + } + return action_type; + } + } + + return action_type; +} + +ActionInterfaceType get_action_interface_type(const std::string & topic_name) +{ + for (auto &[topic_type_enum, postfix] : action_topic_to_postfix_map) { + if (topic_name.length() > postfix.length() && + topic_name.compare(topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) + { + return topic_type_enum; + } + } + + return ActionInterfaceType::Unknown; +} + +std::vector action_name_to_action_interface_names(const std::string & action_name) +{ + std::vector action_topics; + + if (action_name.empty()) { + return action_topics; + } + + for (auto &[topic_type_enum, postfix] : action_topic_to_postfix_map) { + action_topics.push_back(action_name + postfix); + } + + return action_topics; +} +} // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 87ec2bc5f1..902ecfce13 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -24,7 +24,9 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "service_msgs/msg/service_event_info.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/service_utils.hpp" + #include "rosbag2_storage/metadata_io.hpp" #include "rosbag2_storage/storage_factory.hpp" @@ -67,10 +69,83 @@ struct service_req_resp_info std::unordered_map request; std::unordered_map response; }; + +struct action_service_req_resp_info +{ + service_req_resp_info send_goal_service; + service_req_resp_info cancel_goal_service; + service_req_resp_info get_result_service; +}; + +inline size_t calculate_number_of_messages( + std::unordered_map & message_map) +{ + size_t message_count = 0; + for (auto & [client_id, message_list] : message_map) { + message_count += message_list.size(); + } + + return message_count; +} + +using action_analysis = + std::unordered_map>; + +inline void update_action_service_info_with_num_req_resp( + action_analysis & action_process_info, + std::unordered_map> & all_action_info) +{ + for (auto & [action_name, action_info] : action_process_info) { + // Get the number of request from all clients for send_goal + all_action_info[action_name]->send_goal_service_msg_count.first = + calculate_number_of_messages(action_info->send_goal_service.request); + + // Get the number of request from all clients for cancel_goal + all_action_info[action_name]->cancel_goal_service_msg_count.first = + calculate_number_of_messages(action_info->cancel_goal_service.request); + + // Get the number of request from all clients for get_result + all_action_info[action_name]->get_result_service_msg_count.first = + calculate_number_of_messages(action_info->get_result_service.request); + + // Get the number of response from all clients for send_goal + all_action_info[action_name]->send_goal_service_msg_count.second = + calculate_number_of_messages(action_info->send_goal_service.response); + + // Get the number of response from all clients for cancel_goal + all_action_info[action_name]->cancel_goal_service_msg_count.second = + calculate_number_of_messages(action_info->cancel_goal_service.response); + + // Get the number of response from all clients for get_result + all_action_info[action_name]->get_result_service_msg_count.second = + calculate_number_of_messages(action_info->get_result_service.response); + } +} + +using service_analysis = + std::unordered_map>; + +inline void update_service_info_with_num_req_resp( + service_analysis & service_process_info, + std::unordered_map> & all_service_info) +{ + for (auto & [topic_name, service_info] : service_process_info) { + // Get the number of request from all clients + all_service_info[topic_name]->request_count = + calculate_number_of_messages(service_info->request); + + // Get the number of response from all clients + all_service_info[topic_name]->response_count = + calculate_number_of_messages(service_info->response); + } +} } // namespace -std::vector> Info::read_service_info( - const std::string & uri, const std::string & storage_id) +std::pair< + std::vector>, + std::vector> +> +Info::read_service_and_action_info(const std::string & uri, const std::string & storage_id) { rosbag2_storage::StorageFactory factory; auto storage = factory.open_read_only({uri, storage_id}); @@ -83,86 +158,166 @@ std::vector> Info::read_service_info( throw std::runtime_error("Failed to set read order on " + uri); } - using service_analysis = - std::unordered_map>; - - std::unordered_map> all_service_info; + // Service event topic name as key service_analysis service_process_info; + std::unordered_map> all_service_info; + + // Action name as key + action_analysis action_process_info; + std::unordered_map> all_action_info; + + std::vector> output_action_info; + std::vector> output_service_info; + + std::unordered_map action_interface_name_to_action_name_map; + std::unordered_map service_event_name_to_service_name_map; auto all_topics_types = storage->get_all_topics_and_types(); for (auto & t : all_topics_types) { - if (is_service_event_topic(t.name, t.type)) { + if (is_topic_belong_to_action(t.name, t.type)) { + std::shared_ptr action_info; + std::string action_name = action_interface_name_to_action_name(t.name); + if (all_action_info.find(action_name) == all_action_info.end()) { + action_info = std::make_shared(); + action_info->name = action_name; + action_info->serialization_format = t.serialization_format; + all_action_info.emplace(action_name, action_info); + action_process_info[action_name] = std::make_shared(); + } else { + action_info = all_action_info[action_name]; + } + + // Update action type. Note: cancel_goal event topic and status topic cannot get type + if (action_info->type.empty()) { + action_info->type = get_action_type_for_info(t.type); + } + + // Update action_interface_name_to_action_name_map to speed up following code. + action_interface_name_to_action_name_map[t.name] = action_name; + } else if (is_service_event_topic(t.name, t.type)) { auto service_info = std::make_shared(); service_info->name = service_event_topic_name_to_service_name(t.name); service_info->type = service_event_topic_type_to_service_type(t.type); service_info->serialization_format = t.serialization_format; all_service_info.emplace(t.name, service_info); service_process_info[t.name] = std::make_shared(); + + // Update service_event_name_to_service_name_map to speed up following code. + service_event_name_to_service_name_map[t.name] = service_info->name; } } - std::vector> ret_service_info; - - if (!all_service_info.empty()) { + if (!all_service_info.empty() || !all_action_info.empty()) { auto msg = service_msgs::msg::ServiceEventInfo(); - const rosidl_message_type_support_t * type_support_info = + const rosidl_message_type_support_t * service_event_type_support_info = rosidl_typesupport_cpp:: get_message_type_support_handle(); while (storage->has_next()) { auto bag_msg = storage->read_next(); - // Check if topic is service event topic - auto one_service_info = all_service_info.find(bag_msg->topic_name); - if (one_service_info == all_service_info.end()) { + if (action_interface_name_to_action_name_map.count(bag_msg->topic_name) == 0 && + service_event_name_to_service_name_map.count(bag_msg->topic_name) == 0) + { + continue; // Skip the regular topics + } + + auto action_interface_type = get_action_interface_type(bag_msg->topic_name); + if (action_interface_type == ActionInterfaceType::Feedback || + action_interface_type == ActionInterfaceType::Status) + { + auto action_name = action_interface_name_to_action_name_map[bag_msg->topic_name]; + if (action_interface_type == ActionInterfaceType::Feedback) { + all_action_info[action_name]->feedback_topic_msg_count++; + } else { + all_action_info[action_name]->status_topic_msg_count++; + } continue; } auto ret = rmw_deserialize( bag_msg->serialized_data.get(), - type_support_info, - reinterpret_cast(&msg)); + service_event_type_support_info, + reinterpret_cast(&msg) + ); if (ret != RMW_RET_OK) { throw std::runtime_error( "Failed to deserialize message from " + bag_msg->topic_name + " !"); } - switch (msg.event_type) { - case service_msgs::msg::ServiceEventInfo::REQUEST_SENT: - case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: - service_process_info[bag_msg->topic_name]->request[msg.client_gid].emplace( - msg.sequence_number); - break; - case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: - case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: - service_process_info[bag_msg->topic_name]->response[msg.client_gid].emplace( - msg.sequence_number); - break; + if (action_interface_type != ActionInterfaceType::Unknown) { + auto action_name = action_interface_name_to_action_name_map[bag_msg->topic_name]; + auto action = action_process_info[action_name]; + + // Handle action service event topic + switch (msg.event_type) { + case service_msgs::msg::ServiceEventInfo::REQUEST_SENT: + case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: + { + if (action_interface_type == ActionInterfaceType::SendGoalEvent) { + action->send_goal_service.request[msg.client_gid].emplace(msg.sequence_number); + } else if (action_interface_type == ActionInterfaceType::GetResultEvent) { + action->get_result_service.request[msg.client_gid].emplace(msg.sequence_number); + } else { + // TopicsInAction::CancelGoalEvent + action->cancel_goal_service.request[msg.client_gid].emplace(msg.sequence_number); + } + break; + } + case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: + case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: + { + if (action_interface_type == ActionInterfaceType::SendGoalEvent) { + action->send_goal_service.response[msg.client_gid].emplace(msg.sequence_number); + } else if (action_interface_type == ActionInterfaceType::GetResultEvent) { + action->get_result_service.response[msg.client_gid].emplace(msg.sequence_number); + } else { + // TopicsInAction::CancelGoalEvent + action->cancel_goal_service.response[msg.client_gid].emplace(msg.sequence_number); + } + break; + } + default: + throw std::range_error("Invalid service event type " + + std::to_string(msg.event_type) + " !"); + } + } else { + // Handle service event topic + auto service_info = service_process_info[bag_msg->topic_name]; + switch (msg.event_type) { + case service_msgs::msg::ServiceEventInfo::REQUEST_SENT: + case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: + service_info->request[msg.client_gid].emplace(msg.sequence_number); + break; + case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: + case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: + service_info->response[msg.client_gid].emplace(msg.sequence_number); + break; + default: + throw std::range_error("Invalid service event type " + + std::to_string(msg.event_type) + " !"); + } } } - for (auto & [topic_name, service_info] : service_process_info) { - size_t count = 0; - // Get the number of request from all clients - for (auto &[client_id, request_list] : service_info->request) { - count += request_list.size(); - } - all_service_info[topic_name]->request_count = count; + // Process action_process_info to get the number of request and response + update_action_service_info_with_num_req_resp(action_process_info, all_action_info); - count = 0; - // Get the number of response from all clients - for (auto &[client_id, response_list] : service_info->response) { - count += response_list.size(); - } - all_service_info[topic_name]->response_count = count; + // Covert all_action_info to output_action_info + for (auto & [action_name, action_info] : all_action_info) { + output_action_info.emplace_back(std::move(action_info)); } + // Process service_process_info to get the number of request and response + update_service_info_with_num_req_resp(service_process_info, all_service_info); + + // Convert all_service_info to output_service_info for (auto & [topic_name, service_info] : all_service_info) { - ret_service_info.emplace_back(std::move(service_info)); + output_service_info.emplace_back(std::move(service_info)); } } - return ret_service_info; + return std::make_pair(std::move(output_service_info), std::move(output_action_info)); } std::unordered_map Info::compute_messages_size_contribution( diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index 714e9e1e72..6727f2a2cb 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -30,13 +30,16 @@ bool is_service_event_topic(const std::string & topic_name, const std::string & { if (topic_name.length() <= kServiceEventTopicPostfixLen) { return false; - } else { - // The end of the topic name should be "/_service_event" - if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != - RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - { - return false; - } + } + + if (topic_name.find("/_action/") != std::string::npos) { + return false; + } + + if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + return false; } if (topic_type.length() <= kServiceEventTypePostfixLen) { diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index ea6d2fa579..5a82824721 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -29,8 +29,6 @@ #include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/logging.hpp" -#include "rosbag2_cpp/service_utils.hpp" - #include "rosbag2_storage/default_storage_id.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -222,20 +220,13 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic return; } rosbag2_storage::MessageDefinition definition; - - std::string topic_type; - if (is_service_event_topic(topic_with_type.name, topic_with_type.type)) { - // change service event type to service type for next step to get message definition - topic_type = service_event_topic_type_to_service_type(topic_with_type.type); - } else { - topic_type = topic_with_type.type; - } - try { - definition = message_definitions_.get_full_text(topic_type); + definition = message_definitions_.get_full_text(topic_with_type.type); } catch (DefinitionNotFoundError &) { - definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_type); + definition = + rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_with_type.type); } + create_topic(topic_with_type, definition); } diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp new file mode 100644 index 0000000000..a5444a0270 --- /dev/null +++ b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp @@ -0,0 +1,141 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include +#include +#include + +#include "rosbag2_cpp/action_utils.hpp" +#include "rosbag2_test_common/memory_management.hpp" + +using namespace ::testing; // NOLINT +using namespace rosbag2_test_common; // NOLINT + +class ActionUtilsTest : public Test +{ +public: + MemoryManagement memory_management_; +}; + +TEST_F(ActionUtilsTest, check_is_topic_related_to_action) +{ + std::vector, bool>> all_test_data = + { + {{"/abc/_action/feedback", "package/action/xyz_FeedbackMessage"}, true}, + {{"/abc/action/feedback", "package/action/xyz_FeedbackMessage"}, false}, + {{"/abc/_action/feedback", "package/xyz_FeedbackMessage"}, false}, + {{"/abc/_action/feedback", "package/action/xyz"}, false}, + {{"/abc/_action/get_result/_service_event", "package/action/xyz_GetResult_Event"}, true}, + {{"/abc/action/get_result/_service_event", "package/action/xyz_GetResult_Event"}, false}, + {{"/abc/_action/get_result", "package/action/xyz_GetResult_Event"}, false}, + {{"/abc/_action/get_result/service_event", "package/action/xyz_GetResult_Event"}, false}, + {{"/abc/_action/get_result/_service_event", "package/xyz_GetResult_Event"}, false}, + {{"/abc/_action/get_result/_service_event", "package/action/xyz_GetResult"}, false}, + {{"/abc/_action/send_goal/_service_event", "package/action/xyz_SendGoal_Event"}, true}, + {{"/abc/action/send_goal/_service_event", "package/action/xyz_SendGoal_Event"}, false}, + {{"/abc/_action/send_goal", "package/action/xyz_SendGoal_Event"}, false}, + {{"/abc/_action/send_goal/service_event", "package/action/xyz_GetResult_Event"}, false}, + {{"/abc/_action/send_goal/_service_event", "package/xyz_SendGoal_Event"}, false}, + {{"/abc/_action/send_goal/_service_event", "package/action/xyz_SendGoal"}, false}, + {{"/abc/_action/cancel_goal/_service_event", "action_msgs/srv/CancelGoal_Event"}, true}, + {{"/abc/action/cancel_goal/_service_event", "action_msgs/srv/CancelGoal_Event"}, false}, + {{"/abc/_action/cancel_goal", "action_msgs/srv/CancelGoal_Event"}, false}, + {{"/abc/_action/cancel_goal/_service_event", "action_msgs/CancelGoal_Event"}, false}, + {{"/abc/_action/cancel_goal/service_event", "action_msgs/srv/CancelGoal"}, false}, + {{"/abc/_action/status", "action_msgs/msg/GoalStatusArray"}, true}, + {{"/abc/action/status", "action_msgs/msg/GoalStatusArray"}, false}, + {{"/abc/_action/status", "action_msgs/GoalStatusArray"}, false}, + {{"/abc/_action/status", "action_msgs/msg/GoalStatus"}, false} + }; + + for (const auto & [topic_name_and_type, expected_result] : all_test_data) { + const auto & [topic_name, topic_type] = topic_name_and_type; + EXPECT_TRUE(rosbag2_cpp::is_topic_belong_to_action(topic_name, topic_type) == expected_result); + } +} + +TEST_F(ActionUtilsTest, check_action_topic_name_to_action_name) +{ + std::vector> all_test_data = + { + {"/abc/_action/feedback", "/abc"}, + {"/abc/_action/get_result/_service_event", "/abc"}, + {"/abc/_action/send_goal/_service_event", "/abc"}, + {"/abc/_action/cancel_goal/_service_event", "/abc"}, + {"/abc/_action/status", "/abc"}, + {"/_action/status", ""}, + {"/abc/action/status", ""} + }; + + for (const auto & [topic_name, action_name] : all_test_data) { + EXPECT_TRUE(rosbag2_cpp::action_interface_name_to_action_name(topic_name) == action_name); + } +} + +TEST_F(ActionUtilsTest, check_action_topic_type_to_action_type) +{ + std::vector> all_test_data = + { + {"package/action/xyz_FeedbackMessage", "package/action/xyz"}, + {"package/action/xyz_GetResult_Event", "package/action/xyz"}, + {"package/action/xyz_SendGoal_Event", "package/action/xyz"}, + {"action_msgs/srv/CancelGoal_Event", ""}, + {"action_msgs/msg/GoalStatusArray", ""} + }; + + for (const auto & [topic_type, action_type] : all_test_data) { + EXPECT_EQ(rosbag2_cpp::get_action_type_for_info(topic_type), action_type); + } +} + +TEST_F(ActionUtilsTest, check_get_action_interface_type) +{ + std::vector> all_test_data = + { + {"/abc/_action/feedback", rosbag2_cpp::ActionInterfaceType::Feedback}, + {"/abc/_action/get_result/_service_event", rosbag2_cpp::ActionInterfaceType::GetResultEvent}, + {"/abc/_action/send_goal/_service_event", rosbag2_cpp::ActionInterfaceType::SendGoalEvent}, + {"/abc/_action/cancel_goal/_service_event", rosbag2_cpp::ActionInterfaceType::CancelGoalEvent}, + {"/abc/_action/status", rosbag2_cpp::ActionInterfaceType::Status}, + {"/_action/status", rosbag2_cpp::ActionInterfaceType::Unknown}, + {"/abc/action/status", rosbag2_cpp::ActionInterfaceType::Unknown} + }; + + for (const auto & [topic_name, action_interface_type] : all_test_data) { + EXPECT_EQ(rosbag2_cpp::get_action_interface_type(topic_name), action_interface_type); + } +} + +TEST_F(ActionUtilsTest, check_action_name_to_action_topic_name) +{ + std::string action_name = "abc"; + std::vector expected_action_topics = + { + "abc/_action/feedback", + "abc/_action/get_result/_service_event", + "abc/_action/send_goal/_service_event", + "abc/_action/cancel_goal/_service_event", + "abc/_action/status" + }; + + auto output_action_topics = rosbag2_cpp::action_name_to_action_interface_names(action_name); + + for (auto & topic : output_action_topics) { + EXPECT_TRUE(std::find( + expected_action_topics.begin(), + expected_action_topics.end(), + topic) != expected_action_topics.end()); + } +} diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index caac164901..15bdb9463a 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -92,7 +92,9 @@ target_link_libraries(_writer PUBLIC pybind11_add_module(_info src/rosbag2_py/_info.cpp src/rosbag2_py/format_bag_metadata.cpp + src/rosbag2_py/format_action_info.cpp src/rosbag2_py/format_service_info.cpp + src/rosbag2_py/format_utils.cpp src/rosbag2_py/info_sorting_method.cpp ) target_link_libraries(_info PUBLIC diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index 727c469a97..d7dc16ba40 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -64,6 +64,8 @@ class Player: def play(self, storage_options: List[rosbag2_py._storage.StorageOptions], play_options: PlayOptions) -> None: ... class RecordOptions: + actions: List[str] + all_actions: bool all_services: bool all_topics: bool compression_format: str @@ -72,6 +74,7 @@ class RecordOptions: compression_threads: int compression_threads_priority: int disable_keyboard_controls: bool + exclude_actions: List[str] exclude_regex: str exclude_service_events: List[str] exclude_topic_types: List[str] diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 6ae633c47e..7fe3f0b309 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -19,7 +19,9 @@ #include "info_sorting_method.hpp" #include "format_bag_metadata.hpp" +#include "format_action_info.hpp" #include "format_service_info.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/bag_metadata.hpp" @@ -64,7 +66,10 @@ class Info const auto & topic_info = metadata_info.topics_with_message_count[idx]; if (!rosbag2_cpp::is_service_event_topic( topic_info.topic_metadata.name, - topic_info.topic_metadata.type)) + topic_info.topic_metadata.type) && + !rosbag2_cpp::is_topic_belong_to_action( + topic_info.topic_metadata.name, + topic_info.topic_metadata.type)) { std::cout << topic_info.topic_metadata.name << std::endl; } @@ -77,16 +82,14 @@ class Info const std::string & sorting_method) { std::vector> all_services_info; + std::vector> all_actions_info; + for (auto & file_info : metadata_info.files) { - auto services_info = info_->read_service_info( - uri + "/" + file_info.path, - metadata_info.storage_identifier); - if (!services_info.empty()) { - all_services_info.insert( - all_services_info.end(), - services_info.begin(), - services_info.end()); - } + auto [service_info, action_info] = info_->read_service_and_action_info( + uri + "/" + file_info.path, metadata_info.storage_identifier); + + all_services_info.insert(all_services_info.end(), service_info.begin(), service_info.end()); + all_actions_info.insert(all_actions_info.end(), action_info.begin(), action_info.end()); } std::unordered_map messages_size = {}; @@ -100,10 +103,12 @@ class Info } rosbag2_py::InfoSortingMethod sort_method = info_sorting_method_from_string(sorting_method); - // Output formatted metadata and service info + // Output formatted metadata, service info and action info std::cout << format_bag_meta_data(metadata_info, messages_size, true, true, sort_method); std::cout << format_service_info(all_services_info, messages_size, true, sort_method) << std::endl; + std::cout << + format_action_info(all_actions_info, messages_size, true, sort_method) << std::endl; } std::unordered_set get_sorting_methods() diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 1b4489197a..7d9cf09f8f 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -606,6 +606,9 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("services", &RecordOptions::services) .def_readwrite("all_services", &RecordOptions::all_services) .def_readwrite("disable_keyboard_controls", &RecordOptions::disable_keyboard_controls) + .def_readwrite("actions", &RecordOptions::actions) + .def_readwrite("all_actions", &RecordOptions::all_actions) + .def_readwrite("exclude_actions", &RecordOptions::exclude_actions) ; py::class_(m, "Player") diff --git a/rosbag2_py/src/rosbag2_py/action_info.hpp b/rosbag2_py/src/rosbag2_py/action_info.hpp new file mode 100644 index 0000000000..c7e439c7e6 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/action_info.hpp @@ -0,0 +1,43 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef ROSBAG2_PY__ACTION_INFO_HPP_ +#define ROSBAG2_PY__ACTION_INFO_HPP_ + +#include + +namespace rosbag2_py +{ + +struct ActionMetadata +{ + std::string name; + std::string type; + std::string serialization_format; +}; + +struct ActionInformation +{ + ActionMetadata action_metadata; + size_t send_goal_event_message_count = 0; + size_t cancel_goal_event_message_count = 0; + size_t get_result_event_message_count = 0; + size_t feedback_message_count = 0; + size_t status_message_count = 0; +}; + +} // namespace rosbag2_py + +#endif // ROSBAG2_PY__ACTION_INFO_HPP_ diff --git a/rosbag2_py/src/rosbag2_py/format_action_info.cpp b/rosbag2_py/src/rosbag2_py/format_action_info.cpp new file mode 100644 index 0000000000..ba01ce1a24 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_action_info.cpp @@ -0,0 +1,91 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "format_action_info.hpp" +#include "format_utils.hpp" + +#include "rosbag2_cpp/action_utils.hpp" + +namespace rosbag2_py +{ + +std::string +format_action_info( + std::vector> & action_info_list, + const std::unordered_map & messages_size, + bool verbose, + const InfoSortingMethod sort_method) +{ + std::stringstream info_stream; + info_stream << "Actions: " << action_info_list.size() << std::endl; + info_stream << "Action information: "; + + if (action_info_list.empty()) { + return info_stream.str(); + } + + std::unordered_map action_messages_size; + for ( const auto & [topic_name, message_size] : messages_size ) { + auto action_name = rosbag2_cpp::action_interface_name_to_action_name(topic_name); + if (!action_name.empty()) { + if (action_messages_size.find(action_name) == action_messages_size.end()) { + action_messages_size[action_name] = message_size; + } else { + action_messages_size[action_name] += message_size; + } + } + } + + auto print_action_info = + [&info_stream, verbose, &action_messages_size]( + const std::shared_ptr & ai) -> void + { + info_stream << std::endl; + info_stream << " Action: " << ai->name << " | "; + info_stream << "Type: " << ai->type << " | "; + info_stream << "Topics: 2" << " | "; + info_stream << "Service: 3" << " | "; + if (verbose) { + info_stream << "Size Contribution: " << format_file_size(action_messages_size[ai->name]) + << " | "; + } + info_stream << "Serialization Format: " << ai->serialization_format << std::endl; + info_stream << " Topic: feedback | Count: " << ai->feedback_topic_msg_count << std::endl; + info_stream << " Topic: status | Count: " << ai->status_topic_msg_count << std::endl; + info_stream << " Service: send_goal | Request Count: " + << ai->send_goal_service_msg_count.first + << " | Response Count: " + << ai->send_goal_service_msg_count.second << std::endl; + info_stream << " Service: cancel_goal | Request Count: " + << ai->cancel_goal_service_msg_count.first + << " | Response Count: " + << ai->cancel_goal_service_msg_count.second << std::endl; + info_stream << " Service: get_result | Request Count: " + << ai->get_result_service_msg_count.first + << " | Response Count: " << ai->get_result_service_msg_count.second; + }; + + std::vector sorted_idx = generate_sorted_idx(action_info_list, sort_method); + + for (size_t j = 0; j < action_info_list.size(); ++j) { + print_action_info(action_info_list[sorted_idx[j]]); + } + + return info_stream.str(); +} + +} // namespace rosbag2_py diff --git a/rosbag2_py/src/rosbag2_py/format_action_info.hpp b/rosbag2_py/src/rosbag2_py/format_action_info.hpp new file mode 100644 index 0000000000..0844b9adc8 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_action_info.hpp @@ -0,0 +1,37 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_PY__FORMAT_ACTION_INFO_HPP_ +#define ROSBAG2_PY__FORMAT_ACTION_INFO_HPP_ + +#include +#include +#include +#include + +#include "info_sorting_method.hpp" +#include "rosbag2_cpp/info.hpp" + +namespace rosbag2_py +{ + +std::string format_action_info( + std::vector> & action_info, + const std::unordered_map & messages_size, + bool verbose = false, + const InfoSortingMethod sort_method = InfoSortingMethod::NAME); + +} // namespace rosbag2_py + +#endif // ROSBAG2_PY__FORMAT_ACTION_INFO_HPP_ diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index 03a6769c16..3593e50f66 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -25,9 +25,11 @@ #include #endif +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/bag_metadata.hpp" +#include "action_info.hpp" #include "format_bag_metadata.hpp" #include "service_event_info.hpp" @@ -145,11 +147,14 @@ void format_topics_with_type( size_t number_of_topics = topics.size(); size_t i = 0; - // Find first topic which isn't service event topic + // Find first topic which is unrelated to service or action. while (i < number_of_topics && - rosbag2_cpp::is_service_event_topic( + (rosbag2_cpp::is_service_event_topic( topics[sorted_idx[i]].topic_metadata.name, - topics[sorted_idx[i]].topic_metadata.type)) + topics[sorted_idx[i]].topic_metadata.type) || + rosbag2_cpp::is_topic_belong_to_action( + topics[sorted_idx[i]].topic_metadata.name, + topics[sorted_idx[i]].topic_metadata.type))) { i++; } @@ -162,6 +167,8 @@ void format_topics_with_type( print_topic_info(topics[sorted_idx[i]]); for (size_t j = ++i; j < number_of_topics; ++j) { if (rosbag2_cpp::is_service_event_topic( + topics[sorted_idx[j]].topic_metadata.name, topics[sorted_idx[j]].topic_metadata.type) || + rosbag2_cpp::is_topic_belong_to_action( topics[sorted_idx[j]].topic_metadata.name, topics[sorted_idx[j]].topic_metadata.type)) { continue; @@ -171,15 +178,67 @@ void format_topics_with_type( } } +using ServiceInfoList = std::vector>; +using ActionInfoList = std::vector>; +using ActionInfoMap = + std::unordered_map>; -std::vector> filter_service_event_topic( +void filter_service_and_action_info( const std::vector & topics_with_message_count, - size_t & total_service_event_msg_count) + ServiceInfoList & service_info_list, + size_t & total_service_event_msg_count, + ActionInfoList & action_info_list, + size_t & total_action_msg_count) { total_service_event_msg_count = 0; - std::vector> service_info_list; + total_action_msg_count = 0; + + ActionInfoMap action_info_map; for (auto & topic : topics_with_message_count) { + const auto action_interface_type = + rosbag2_cpp::get_action_interface_type(topic.topic_metadata.name); + if (rosbag2_cpp::is_topic_belong_to_action(action_interface_type, topic.topic_metadata.type)) { + auto action_name = + rosbag2_cpp::action_interface_name_to_action_name(topic.topic_metadata.name); + + if (action_info_map.find(action_name) == action_info_map.end()) { + action_info_map[action_name] = std::make_shared(); + action_info_map[action_name]->action_metadata.name = action_name; + action_info_map[action_name]->action_metadata.serialization_format = + topic.topic_metadata.serialization_format; + } + + // If the cancel_goal or status message is processed first, it can result in the type + // being empty. So If the type is empty, it will be updated with subsequent messages. + if (action_info_map[action_name]->action_metadata.type.empty()) { + action_info_map[action_name]->action_metadata.type = + rosbag2_cpp::get_action_type_for_info(topic.topic_metadata.type); + } + + switch (action_interface_type) { + case rosbag2_cpp::ActionInterfaceType::SendGoalEvent: + action_info_map[action_name]->send_goal_event_message_count = topic.message_count; + break; + case rosbag2_cpp::ActionInterfaceType::CancelGoalEvent: + action_info_map[action_name]->cancel_goal_event_message_count = topic.message_count; + break; + case rosbag2_cpp::ActionInterfaceType::GetResultEvent: + action_info_map[action_name]->get_result_event_message_count = topic.message_count; + break; + case rosbag2_cpp::ActionInterfaceType::Feedback: + action_info_map[action_name]->feedback_message_count = topic.message_count; + break; + case rosbag2_cpp::ActionInterfaceType::Status: + action_info_map[action_name]->status_message_count = topic.message_count; + break; + default: // Never go here + throw std::out_of_range("Invalid action interface type"); + } + total_action_msg_count += topic.message_count; + continue; + } + if (rosbag2_cpp::is_service_event_topic( topic.topic_metadata.name, topic.topic_metadata.type)) { @@ -196,7 +255,10 @@ std::vector> filter_service } } - return service_info_list; + // Convert action_info_map to action_info_list + for (auto & action_info : action_info_map) { + action_info_list.emplace_back(action_info.second); + } } void format_service_with_type( @@ -241,6 +303,44 @@ void format_service_with_type( } } +void format_action_with_type( + const std::vector> & actions, + std::stringstream & info_stream, + const rosbag2_py::InfoSortingMethod sort_method = rosbag2_py::InfoSortingMethod::NAME) +{ + info_stream << std::endl; + if (actions.empty()) { + return; + } + + auto print_action_info = + [&info_stream](const std::shared_ptr & ai) -> void { + info_stream << " Action: " << ai->action_metadata.name << " | "; + info_stream << "Type: " << ai->action_metadata.type << " | "; + info_stream << "Topics: 2" << " | "; + info_stream << "Service: 3" << " | "; + info_stream << "Serialization Format: " + << ai->action_metadata.serialization_format << std::endl; + info_stream << " Topic: feedback | Count: " << ai->feedback_message_count << std::endl; + info_stream << " Topic: status | Count: " << ai->status_message_count << std::endl; + info_stream << " Service: send_goal | Event Count: " + << ai->get_result_event_message_count << std::endl; + info_stream << " Service: cancel_goal | Event Count: " + << ai->cancel_goal_event_message_count << std::endl; + info_stream << " Service: get_result | Event Count: " + << ai->get_result_event_message_count; + }; + + std::vector sorted_idx = rosbag2_py::generate_sorted_idx(actions, sort_method); + + print_action_info(actions[sorted_idx[0]]); + auto number_of_services = actions.size(); + for (size_t j = 1; j < number_of_services; ++j) { + info_stream << std::endl; + print_action_info(actions[sorted_idx[j]]); + } +} + } // namespace namespace rosbag2_py @@ -262,9 +362,17 @@ std::string format_bag_meta_data( ros_distro = "unknown"; } + ServiceInfoList service_info_list; size_t total_service_event_msg_count = 0; - std::vector> service_info_list = - filter_service_event_topic(metadata.topics_with_message_count, total_service_event_msg_count); + ActionInfoList action_info_list; + size_t total_action_msg_count = 0; + + filter_service_and_action_info( + metadata.topics_with_message_count, + service_info_list, + total_service_event_msg_count, + action_info_list, + total_action_msg_count); info_stream << std::endl; info_stream << "Files: "; @@ -278,8 +386,9 @@ std::string format_bag_meta_data( info_stream << "Start: " << format_time_point(start_time) << std::endl; info_stream << "End: " << format_time_point(end_time) << std::endl; - info_stream << "Messages: " << metadata.message_count - total_service_event_msg_count << - std::endl; + info_stream << "Messages: " + << metadata.message_count - total_service_event_msg_count - total_action_msg_count + << std::endl; info_stream << "Topic information: "; format_topics_with_type( metadata.topics_with_message_count, @@ -288,7 +397,7 @@ std::string format_bag_meta_data( sort_method); if (!only_topic) { - info_stream << "Service: " << service_info_list.size() << std::endl; + info_stream << "Services: " << service_info_list.size() << std::endl; info_stream << "Service information: "; if (!service_info_list.empty()) { format_service_with_type( @@ -298,6 +407,16 @@ std::string format_bag_meta_data( info_stream, indentation_spaces + 2, sort_method); + } else { + info_stream << std::endl; + } + info_stream << "Actions: " << action_info_list.size() << std::endl; + info_stream << "Action information: "; + if (!action_info_list.empty()) { + format_action_with_type( + action_info_list, + info_stream, + sort_method); } } diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index 814076b83b..34185b98f7 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -15,30 +15,9 @@ #include #include "format_service_info.hpp" -#include "rosbag2_cpp/service_utils.hpp" - -namespace -{ - -std::string format_file_size(uint64_t file_size) -{ - double size = static_cast(file_size); - static const char * units[] = {"B", "KiB", "MiB", "GiB", "TiB"}; - double reference_number_bytes = 1024; - int index = 0; - while (size >= reference_number_bytes && index < 4) { - size /= reference_number_bytes; - index++; - } - - std::stringstream rounded_size; - int size_format_precision = index == 0 ? 0 : 1; - rounded_size << std::setprecision(size_format_precision) << std::fixed << size; - return rounded_size.str() + " " + units[index]; -} - -} // namespace +#include "format_utils.hpp" +#include "rosbag2_cpp/service_utils.hpp" namespace rosbag2_py { @@ -52,7 +31,7 @@ format_service_info( std::stringstream info_stream; const std::string service_info_string = "Service information: "; auto indentation_spaces = service_info_string.size(); - info_stream << "Service: " << service_info_list.size() << std::endl; + info_stream << "Services: " << service_info_list.size() << std::endl; info_stream << service_info_string; if (service_info_list.empty()) { @@ -76,7 +55,6 @@ format_service_info( info_stream << "Size Contribution: " << format_file_size(service_size) << " | "; } info_stream << "Serialization Format: " << si->serialization_format; - info_stream << std::endl; }; std::vector sorted_idx = generate_sorted_idx(service_info_list, sort_method); @@ -84,6 +62,7 @@ format_service_info( print_service_info(service_info_list[sorted_idx[0]]); auto number_of_services = service_info_list.size(); for (size_t j = 1; j < number_of_services; ++j) { + info_stream << std::endl; info_stream << std::string(indentation_spaces, ' '); print_service_info(service_info_list[sorted_idx[j]]); } diff --git a/rosbag2_py/src/rosbag2_py/format_utils.cpp b/rosbag2_py/src/rosbag2_py/format_utils.cpp new file mode 100644 index 0000000000..1bce0b17a4 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_utils.cpp @@ -0,0 +1,40 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "format_utils.hpp" + +namespace rosbag2_py +{ +std::string format_file_size(uint64_t file_size) +{ + double size = static_cast(file_size); + static const char * units[] = {"B", "KiB", "MiB", "GiB", "TiB"}; + double reference_number_bytes = 1024; + int index = 0; + while (size >= reference_number_bytes && index < 4) { + size /= reference_number_bytes; + index++; + } + + std::stringstream rounded_size; + int size_format_precision = index == 0 ? 0 : 1; + rounded_size << std::setprecision(size_format_precision) << std::fixed << size; + return rounded_size.str() + " " + units[index]; +} +} // namespace rosbag2_py diff --git a/rosbag2_py/src/rosbag2_py/format_utils.hpp b/rosbag2_py/src/rosbag2_py/format_utils.hpp new file mode 100644 index 0000000000..35de757968 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_utils.hpp @@ -0,0 +1,26 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_PY__FORMAT_UTILS_HPP_ +#define ROSBAG2_PY__FORMAT_UTILS_HPP_ + +#include +#include + +namespace rosbag2_py +{ +std::string format_file_size(uint64_t file_size); +} // namespace rosbag2_py + +#endif // ROSBAG2_PY__FORMAT_UTILS_HPP_ diff --git a/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp b/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp index 6a6e9aace3..45c4d8e8a3 100644 --- a/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp +++ b/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp @@ -126,4 +126,93 @@ std::vector generate_sorted_idx( return sorted_idx; } +std::vector generate_sorted_idx( + const std::vector> & actions, + const InfoSortingMethod sort_method) +{ + std::vector sorted_idx(actions.size()); + std::iota(sorted_idx.begin(), sorted_idx.end(), 0); + std::sort( + sorted_idx.begin(), + sorted_idx.end(), + [&actions, sort_method](size_t i1, size_t i2) { + bool is_greater = false; + switch (sort_method) { + case InfoSortingMethod::NAME: + is_greater = actions[i1]->action_metadata.name < actions[i2]->action_metadata.name; + break; + case InfoSortingMethod::TYPE: + is_greater = actions[i1]->action_metadata.type < actions[i2]->action_metadata.type; + break; + case InfoSortingMethod::COUNT: { + size_t total_count_action1 = actions[i1]->send_goal_event_message_count + + actions[i1]->cancel_goal_event_message_count + + actions[i1]->get_result_event_message_count + + actions[i1]->feedback_message_count + + actions[i1]->status_message_count; + size_t total_count_action2 = actions[i2]->send_goal_event_message_count + + actions[i2]->cancel_goal_event_message_count + + actions[i2]->get_result_event_message_count + + actions[i2]->feedback_message_count + + actions[i2]->status_message_count; + is_greater = total_count_action1 < total_count_action2; + break; + } + default: + throw std::runtime_error("Sorting actions switch is not exhaustive"); + } + return is_greater; + } + ); + return sorted_idx; +} + +std::vector generate_sorted_idx( + const std::vector> & actions, + const InfoSortingMethod sort_method) +{ + std::vector sorted_idx(actions.size()); + std::iota(sorted_idx.begin(), sorted_idx.end(), 0); + std::sort( + sorted_idx.begin(), + sorted_idx.end(), + [&actions, sort_method](size_t i1, size_t i2) { + bool is_greater = false; + switch (sort_method) { + case InfoSortingMethod::NAME: + is_greater = actions[i1]->name < actions[i2]->name; + break; + case InfoSortingMethod::TYPE: + is_greater = actions[i1]->type < actions[i2]->type; + break; + case InfoSortingMethod::COUNT: { + size_t total_count_action1 = + actions[i1]->send_goal_service_msg_count.first + + actions[i1]->send_goal_service_msg_count.second + + actions[i1]->cancel_goal_service_msg_count.first + + actions[i1]->cancel_goal_service_msg_count.second + + actions[i1]->get_result_service_msg_count.first + + actions[i1]->get_result_service_msg_count.second + + actions[i1]->feedback_topic_msg_count + actions[i1]->status_topic_msg_count; + + size_t total_count_action2 = + actions[i2]->send_goal_service_msg_count.first + + actions[i2]->send_goal_service_msg_count.second + + actions[i2]->cancel_goal_service_msg_count.first + + actions[i2]->cancel_goal_service_msg_count.second + + actions[i2]->get_result_service_msg_count.first + + actions[i2]->get_result_service_msg_count.second + + actions[i2]->feedback_topic_msg_count + actions[i2]->status_topic_msg_count; + is_greater = total_count_action1 < total_count_action2; + break; + } + default: + throw std::runtime_error("Sorting actions switch is not exhaustive"); + } + return is_greater; + } + ); + return sorted_idx; +} + } // namespace rosbag2_py diff --git a/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp b/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp index 7f3ef4bcf0..341264b9d5 100644 --- a/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp +++ b/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp @@ -27,6 +27,8 @@ #include "rosbag2_storage/topic_metadata.hpp" #include "rosbag2_storage/bag_metadata.hpp" #include "rosbag2_cpp/info.hpp" + +#include "action_info.hpp" #include "service_event_info.hpp" namespace rosbag2_py @@ -59,6 +61,13 @@ std::vector generate_sorted_idx( const std::vector> & services, InfoSortingMethod sort_method = InfoSortingMethod::NAME); +std::vector generate_sorted_idx( + const std::vector> & actions, + const InfoSortingMethod sort_method = InfoSortingMethod::NAME); + +std::vector generate_sorted_idx( + const std::vector> & services, + InfoSortingMethod sort_method = InfoSortingMethod::NAME); } // namespace rosbag2_py #endif // ROSBAG2_PY__INFO_SORTING_METHOD_HPP_ diff --git a/rosbag2_test_common/CMakeLists.txt b/rosbag2_test_common/CMakeLists.txt index 0d09759fc5..d1bb52f78f 100644 --- a/rosbag2_test_common/CMakeLists.txt +++ b/rosbag2_test_common/CMakeLists.txt @@ -23,6 +23,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rcutils REQUIRED) find_package(rcpputils REQUIRED) find_package(test_msgs REQUIRED) @@ -33,6 +34,7 @@ target_include_directories(${PROJECT_NAME} INTERFACE $) target_link_libraries(${PROJECT_NAME} INTERFACE rclcpp::rclcpp + rclcpp_action::rclcpp_action rcutils::rcutils ${test_msgs_TARGETS} ) @@ -51,7 +53,7 @@ endif() ament_python_install_package(${PROJECT_NAME}) -ament_export_dependencies(rclcpp rcutils test_msgs) +ament_export_dependencies(rclcpp rclcpp_action rcutils test_msgs) # Export old-style CMake variables ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp new file mode 100644 index 0000000000..b135b53556 --- /dev/null +++ b/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp @@ -0,0 +1,248 @@ +// Copyright 2025 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_TEST_COMMON__ACTION_CLIENT_MANAGER_HPP_ +#define ROSBAG2_TEST_COMMON__ACTION_CLIENT_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/service_introspection.h" + +#include "rclcpp/rclcpp.hpp" // rclcpp must be included before the Windows specific includes. +#include "rclcpp_action/rclcpp_action.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace rosbag2_test_common +{ +template +class ActionClientManager : public rclcpp::Node +{ +public: + using Fibonacci = test_msgs::action::Fibonacci; + using ServerGoalHandleFibonacci = rclcpp_action::ServerGoalHandle; + using ClientGoalHandleFibonacci = rclcpp_action::ClientGoalHandle; + + explicit ActionClientManager( + std::string action_name, + std::chrono::seconds exec_goal_time = 1s, + size_t number_of_clients = 1, + bool enable_action_server_introspection = true, + bool enable_action_client_introspection = false) + : Node("action_client_manager_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions().start_parameter_services(false).start_parameter_event_publisher( + false).enable_rosout(false)), + action_name_(std::move(action_name)), + number_of_clients_(number_of_clients), + enable_action_server_introspection_(enable_action_server_introspection), + enable_action_client_introspection_(enable_action_client_introspection) + { + create_action_server(exec_goal_time); + + create_action_client(number_of_clients); + + rcl_service_introspection_state_t introspection_state; + if (enable_action_server_introspection_) { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } else { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } + action_server_->configure_introspection( + get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + + if (enable_action_client_introspection_) { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } else { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } + + for (auto & action_client : action_clients_) { + action_client->configure_introspection( + get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + } + } + + void create_action_server(std::chrono::seconds exec_goal_time) + { + auto handle_goal = [this]( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handle_cancel = [this]( + const std::shared_ptr goal_handle) + { + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this, exec_goal_time]( + const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, + // so we declare a lambda function to be called inside a new thread + auto execute_in_thread = + [this, goal_handle, exec_goal_time]() { + return this->execute(goal_handle, exec_goal_time); + }; + std::thread{execute_in_thread}.detach(); + }; + + action_server_ = rclcpp_action::create_server( + this, + action_name_, + handle_goal, + handle_cancel, + handle_accepted); + } + + void create_action_client(size_t number_of_clients) + { + for (size_t i = 0; i < number_of_clients_; i++) { + auto action_client = rclcpp_action::create_client( + this->get_node_base_interface(), + this->get_node_graph_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + action_name_); + + action_clients_.emplace_back(action_client); + } + } + + bool check_action_server_ready() + { + for (auto & action_client : action_clients_) { + if (!action_client->action_server_is_ready()) { + return false; + } + } + return true; + } + + bool wait_for_action_server_to_be_ready( + std::chrono::duration timeout = std::chrono::seconds(5)) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!check_action_server_ready() && (clock::now() - start) < timeout) { + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + } + return check_action_server_ready(); + } + + bool send_goal( + bool cancel_goal_after_accept = false, + std::chrono::duration timeout = std::chrono::seconds(10)) + { + if (!check_action_server_ready()) { + return false; + } + + for (auto & action_client : action_clients_) { + auto goal_msg = Fibonacci::Goal(); + goal_msg.order = 3; + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.goal_response_callback = [this, action_client, cancel_goal_after_accept]( + const ClientGoalHandleFibonacci::SharedPtr & goal_handle) + { + if (goal_handle) { + if (cancel_goal_after_accept) { + action_client->async_cancel_goal(goal_handle); + } + } + }; + + send_goal_options.feedback_callback = [this]( + ClientGoalHandleFibonacci::SharedPtr, + const std::shared_ptr feedback) + { + (void)feedback; + }; + + send_goal_options.result_callback = [this]( + const ClientGoalHandleFibonacci::WrappedResult & result) + { + (void)result; + }; + + auto send_goal_future = action_client->async_send_goal(goal_msg, send_goal_options); + + if (rclcpp::executors::spin_node_until_future_complete( + exec_, get_node_base_interface(), send_goal_future, 1s) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + + auto goal_handle = send_goal_future.get(); + if (!goal_handle) { + return false; + } + + auto result_future = action_client->async_get_result(goal_handle); + if (rclcpp::executors::spin_node_until_future_complete( + exec_, get_node_base_interface(), result_future, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + } + return true; + } + +private: + rclcpp::executors::SingleThreadedExecutor exec_; + rclcpp_action::Server::SharedPtr action_server_; + std::vector::SharedPtr> action_clients_; + const std::string action_name_; + size_t number_of_clients_; + bool enable_action_server_introspection_; + bool enable_action_client_introspection_; + + void execute( + const std::shared_ptr goal_handle, + std::chrono::seconds exec_goal_time) + { + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + goal_handle->publish_feedback(feedback); + } + + std::this_thread::sleep_for(exec_goal_time); + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + return; + } + + // goal is done + goal_handle->succeed(result); + } +}; +} // namespace rosbag2_test_common + +#endif // ROSBAG2_TEST_COMMON__ACTION_CLIENT_MANAGER_HPP_ diff --git a/rosbag2_test_common/package.xml b/rosbag2_test_common/package.xml index 6d839b5933..52af233a7d 100644 --- a/rosbag2_test_common/package.xml +++ b/rosbag2_test_common/package.xml @@ -15,11 +15,13 @@ ament_cmake_python rclcpp + rclcpp_action rcpputils rcutils test_msgs rclcpp + rclcpp_action rcpputils rcutils test_msgs diff --git a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap deleted file mode 100644 index e68cdbe4b7..0000000000 Binary files a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap and /dev/null differ diff --git a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml deleted file mode 100644 index 9170884e3a..0000000000 --- a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml +++ /dev/null @@ -1,57 +0,0 @@ -rosbag2_bagfile_information: - version: 8 - storage_identifier: mcap - duration: - nanoseconds: 70653944 - starting_time: - nanoseconds_since_epoch: 1699345836270074454 - message_count: 10 - topics_with_message_count: - - topic_metadata: - name: /events/write_split - type: rosbag2_interfaces/msg/WriteSplitEvent - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_5ef58f7106a5cff8f5a794028c18117ee21015850ddcc567df449f41932960f8 - message_count: 0 - - topic_metadata: - name: /test_service1/_service_event - type: test_msgs/srv/BasicTypes_Event - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_5b9988a4805a06ae22dff12903a774ccda710e9514926b826ffc884e3a9a982e - message_count: 4 - - topic_metadata: - name: /test_service2/_service_event - type: test_msgs/srv/BasicTypes_Event - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_5b9988a4805a06ae22dff12903a774ccda710e9514926b826ffc884e3a9a982e - message_count: 4 - - topic_metadata: - name: /test_topic1 - type: test_msgs/msg/Strings - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_eee55533636293ac7c94c62432eedb5777eb849847d2a17b4dcead8c4a20ab32 - message_count: 1 - - topic_metadata: - name: /test_topic2 - type: test_msgs/msg/Strings - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_eee55533636293ac7c94c62432eedb5777eb849847d2a17b4dcead8c4a20ab32 - message_count: 1 - compression_format: "" - compression_mode: "" - relative_file_paths: - - bag_with_topics_and_service_events.mcap - files: - - path: bag_with_topics_and_service_events.mcap - starting_time: - nanoseconds_since_epoch: 1699345836270074454 - duration: - nanoseconds: 70653944 - message_count: 10 - custom_data: ~ - ros_distro: "" \ No newline at end of file diff --git a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.mcap b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.mcap new file mode 100644 index 0000000000..0f1a122914 Binary files /dev/null and b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.mcap differ diff --git a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/metadata.yaml b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/metadata.yaml new file mode 100644 index 0000000000..25bc66175d --- /dev/null +++ b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/metadata.yaml @@ -0,0 +1,411 @@ +rosbag2_bagfile_information: + version: 9 + storage_identifier: mcap + duration: + nanoseconds: 47335781959 + starting_time: + nanoseconds_since_epoch: 1741940193171291565 + message_count: 218 + topics_with_message_count: + - topic_metadata: + name: /test_topic2 + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 34 + - topic_metadata: + name: /test_topic1 + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 37 + - topic_metadata: + name: /test_service2/_service_event + type: example_interfaces/srv/AddTwoInts_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_32c1d140259c71e5c355115942bcb31df98b4330e4d906b1b75ccb1c9b3ce6c8 + message_count: 2 + - topic_metadata: + name: /test_service1/_service_event + type: example_interfaces/srv/AddTwoInts_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_32c1d140259c71e5c355115942bcb31df98b4330e4d906b1b75ccb1c9b3ce6c8 + message_count: 2 + - topic_metadata: + name: /events/write_split + type: rosbag2_interfaces/msg/WriteSplitEvent + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_4a50733d58c48fe31919ae8c8516beda7326d70c8e99aee98af3fdfd44ec5ae6 + message_count: 0 + - topic_metadata: + name: /test_action1/_action/status + type: action_msgs/msg/GoalStatusArray + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_6c1684b00f177d37438febe6e709fc4e2b0d4248dca4854946f9ed8b30cda83e + message_count: 2 + - topic_metadata: + name: /parameter_events + type: rcl_interfaces/msg/ParameterEvent + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_043e627780fcad87a22d225bc2a037361dba713fca6a6b9f4b869a5aa0393204 + message_count: 7 + - topic_metadata: + name: /test_action1/_action/feedback + type: example_interfaces/action/Fibonacci_FeedbackMessage + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_c1de71afd52e49a89c53d8262366884185bc0a02f78ce051c4e46b0a7fe59bb2 + message_count: 5 + - topic_metadata: + name: /test_action1/_action/get_result/_service_event + type: example_interfaces/action/Fibonacci_GetResult_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_ac4e9cdb954c2e192c9f0753d46f59ab6f0101d5fb5b9542fd8ab2906e40c4d9 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/status + type: action_msgs/msg/GoalStatusArray + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_6c1684b00f177d37438febe6e709fc4e2b0d4248dca4854946f9ed8b30cda83e + message_count: 2 + - topic_metadata: + name: /test_action2/_action/cancel_goal/_service_event + type: action_msgs/srv/CancelGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_178f81615ce6be41ad328211d7a56b08b00e73f41f0b8aec256b2f3ff97a5e7e + message_count: 0 + - topic_metadata: + name: /rosout + type: rcl_interfaces/msg/Log + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 10 + nsec: 0 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_e28ce254ca8abc06abf92773b74602cdbf116ed34fbaf294fb9f81da9f318eac + message_count: 112 + - topic_metadata: + name: /test_action1/_action/cancel_goal/_service_event + type: action_msgs/srv/CancelGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_178f81615ce6be41ad328211d7a56b08b00e73f41f0b8aec256b2f3ff97a5e7e + message_count: 0 + - topic_metadata: + name: /test_action2/_action/get_result/_service_event + type: example_interfaces/action/Fibonacci_GetResult_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_ac4e9cdb954c2e192c9f0753d46f59ab6f0101d5fb5b9542fd8ab2906e40c4d9 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/feedback + type: example_interfaces/action/Fibonacci_FeedbackMessage + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_c1de71afd52e49a89c53d8262366884185bc0a02f78ce051c4e46b0a7fe59bb2 + message_count: 7 + - topic_metadata: + name: /test_action1/_action/send_goal/_service_event + type: example_interfaces/action/Fibonacci_SendGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_d5e5b312dbf847307b0e876c2ecc9d80258e95b0d17f8f7bfc581c4a29c83484 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/send_goal/_service_event + type: example_interfaces/action/Fibonacci_SendGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_d5e5b312dbf847307b0e876c2ecc9d80258e95b0d17f8f7bfc581c4a29c83484 + message_count: 2 + compression_format: "" + compression_mode: "" + relative_file_paths: + - bag_with_topics_and_service_events_and_action.mcap + files: + - path: bag_with_topics_and_service_events_and_action.mcap + starting_time: + nanoseconds_since_epoch: 1741940193171291565 + duration: + nanoseconds: 47335781959 + message_count: 218 + custom_data: ~ + ros_distro: rolling diff --git a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 deleted file mode 100644 index 03f1dd9b4d..0000000000 Binary files a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 and /dev/null differ diff --git a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml deleted file mode 100644 index d0e796c5b1..0000000000 --- a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml +++ /dev/null @@ -1,57 +0,0 @@ -rosbag2_bagfile_information: - version: 8 - storage_identifier: sqlite3 - duration: - nanoseconds: 70633730 - starting_time: - nanoseconds_since_epoch: 1699345836023194036 - message_count: 10 - topics_with_message_count: - - topic_metadata: - name: /events/write_split - type: rosbag2_interfaces/msg/WriteSplitEvent - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_5ef58f7106a5cff8f5a794028c18117ee21015850ddcc567df449f41932960f8 - message_count: 0 - - topic_metadata: - name: /test_service1/_service_event - type: test_msgs/srv/BasicTypes_Event - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_5b9988a4805a06ae22dff12903a774ccda710e9514926b826ffc884e3a9a982e - message_count: 4 - - topic_metadata: - name: /test_service2/_service_event - type: test_msgs/srv/BasicTypes_Event - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_5b9988a4805a06ae22dff12903a774ccda710e9514926b826ffc884e3a9a982e - message_count: 4 - - topic_metadata: - name: /test_topic1 - type: test_msgs/msg/Strings - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_eee55533636293ac7c94c62432eedb5777eb849847d2a17b4dcead8c4a20ab32 - message_count: 1 - - topic_metadata: - name: /test_topic2 - type: test_msgs/msg/Strings - serialization_format: cdr - offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" - type_description_hash: RIHS01_eee55533636293ac7c94c62432eedb5777eb849847d2a17b4dcead8c4a20ab32 - message_count: 1 - compression_format: "" - compression_mode: "" - relative_file_paths: - - bag_with_topics_and_service_events.db3 - files: - - path: bag_with_topics_and_service_events.db3 - starting_time: - nanoseconds_since_epoch: 1699345836023194036 - duration: - nanoseconds: 70633730 - message_count: 10 - custom_data: ~ - ros_distro: "" \ No newline at end of file diff --git a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.db3 b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.db3 new file mode 100644 index 0000000000..712aa9aa3f Binary files /dev/null and b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.db3 differ diff --git a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/metadata.yaml b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/metadata.yaml new file mode 100644 index 0000000000..bb2df8d05f --- /dev/null +++ b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/metadata.yaml @@ -0,0 +1,456 @@ +rosbag2_bagfile_information: + version: 9 + storage_identifier: sqlite3 + duration: + nanoseconds: 52559897611 + starting_time: + nanoseconds_since_epoch: 1741949479681027429 + message_count: 187 + topics_with_message_count: + - topic_metadata: + name: /test_topic2 + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 27 + - topic_metadata: + name: /test_topic1 + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 32 + - topic_metadata: + name: /test_action1/_action/status + type: action_msgs/msg/GoalStatusArray + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_6c1684b00f177d37438febe6e709fc4e2b0d4248dca4854946f9ed8b30cda83e + message_count: 2 + - topic_metadata: + name: /test_action1/_action/feedback + type: example_interfaces/action/Fibonacci_FeedbackMessage + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_c1de71afd52e49a89c53d8262366884185bc0a02f78ce051c4e46b0a7fe59bb2 + message_count: 5 + - topic_metadata: + name: /events/write_split + type: rosbag2_interfaces/msg/WriteSplitEvent + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_4a50733d58c48fe31919ae8c8516beda7326d70c8e99aee98af3fdfd44ec5ae6 + message_count: 0 + - topic_metadata: + name: /parameter_events + type: rcl_interfaces/msg/ParameterEvent + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_043e627780fcad87a22d225bc2a037361dba713fca6a6b9f4b869a5aa0393204 + message_count: 0 + - topic_metadata: + name: /test_service1/_service_event + type: example_interfaces/srv/AddTwoInts_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_32c1d140259c71e5c355115942bcb31df98b4330e4d906b1b75ccb1c9b3ce6c8 + message_count: 2 + - topic_metadata: + name: /test_service2/_service_event + type: example_interfaces/srv/AddTwoInts_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_32c1d140259c71e5c355115942bcb31df98b4330e4d906b1b75ccb1c9b3ce6c8 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/status + type: action_msgs/msg/GoalStatusArray + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_6c1684b00f177d37438febe6e709fc4e2b0d4248dca4854946f9ed8b30cda83e + message_count: 2 + - topic_metadata: + name: /test_action1/_action/send_goal/_service_event + type: example_interfaces/action/Fibonacci_SendGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_d5e5b312dbf847307b0e876c2ecc9d80258e95b0d17f8f7bfc581c4a29c83484 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/send_goal/_service_event + type: example_interfaces/action/Fibonacci_SendGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_d5e5b312dbf847307b0e876c2ecc9d80258e95b0d17f8f7bfc581c4a29c83484 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/feedback + type: example_interfaces/action/Fibonacci_FeedbackMessage + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: volatile + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_c1de71afd52e49a89c53d8262366884185bc0a02f78ce051c4e46b0a7fe59bb2 + message_count: 7 + - topic_metadata: + name: /rosout + type: rcl_interfaces/msg/Log + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 10 + nsec: 0 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_e28ce254ca8abc06abf92773b74602cdbf116ed34fbaf294fb9f81da9f318eac + message_count: 100 + - topic_metadata: + name: /test_action1/_action/cancel_goal/_service_event + type: action_msgs/srv/CancelGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_178f81615ce6be41ad328211d7a56b08b00e73f41f0b8aec256b2f3ff97a5e7e + message_count: 0 + - topic_metadata: + name: /test_action2/_action/get_result/_service_event + type: example_interfaces/action/Fibonacci_GetResult_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_ac4e9cdb954c2e192c9f0753d46f59ab6f0101d5fb5b9542fd8ab2906e40c4d9 + message_count: 2 + - topic_metadata: + name: /test_action2/_action/cancel_goal/_service_event + type: action_msgs/srv/CancelGoal_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_178f81615ce6be41ad328211d7a56b08b00e73f41f0b8aec256b2f3ff97a5e7e + message_count: 0 + - topic_metadata: + name: /test_action1/_action/get_result/_service_event + type: example_interfaces/action/Fibonacci_GetResult_Event + serialization_format: cdr + offered_qos_profiles: + - history: unknown + depth: 0 + reliability: reliable + durability: transient_local + deadline: + sec: 9223372036 + nsec: 854775807 + lifespan: + sec: 9223372036 + nsec: 854775807 + liveliness: automatic + liveliness_lease_duration: + sec: 9223372036 + nsec: 854775807 + avoid_ros_namespace_conventions: false + type_description_hash: RIHS01_ac4e9cdb954c2e192c9f0753d46f59ab6f0101d5fb5b9542fd8ab2906e40c4d9 + message_count: 2 + compression_format: "" + compression_mode: "" + relative_file_paths: + - bag_with_topics_and_service_events_and_action.db3 + files: + - path: bag_with_topics_and_service_events_and_action.db3 + starting_time: + nanoseconds_since_epoch: 1741949479681027429 + duration: + nanoseconds: 52559897611 + message_count: 187 + custom_data: ~ + ros_distro: rolling diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp index 401deb311e..3801f443c1 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp @@ -182,12 +182,15 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_only) } rosbag2_cpp::Info info; - std::vector> ret_service_infos; + std::pair>, + std::vector>> + ret_service_action_infos; const std::string recorded_bag_uri = bag_path_str + "/" + get_bag_file_name(); - ASSERT_NO_THROW(ret_service_infos = info.read_service_info(recorded_bag_uri, storage_id)) << - recorded_bag_uri; + ASSERT_NO_THROW( + ret_service_action_infos = info.read_service_and_action_info( + recorded_bag_uri, storage_id)) << recorded_bag_uri; - EXPECT_TRUE(ret_service_infos.empty()); + EXPECT_TRUE(ret_service_action_infos.first.empty()); } TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_services_only) { @@ -206,7 +209,7 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_services_only storage_options.storage_id = storage_id; storage_options.uri = bag_path_str; rosbag2_transport::RecordOptions record_options = - {false, true, false, {}, {}, {}, {"/rosout"}, {}, {}, "cdr", 100ms}; + {false, true, false, false, {}, {}, {}, {}, {"/rosout"}, {}, {}, {}, "cdr", 100ms}; auto recorder = std::make_shared( std::move(writer), storage_options, record_options); recorder->record(); @@ -243,18 +246,21 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_services_only } rosbag2_cpp::Info info; - std::vector> ret_service_infos; + std::pair>, + std::vector>> + ret_service_action_infos; const std::string recorded_bag_uri = bag_path_str + "/" + get_bag_file_name(); - ASSERT_NO_THROW(ret_service_infos = info.read_service_info(recorded_bag_uri, storage_id)) << - recorded_bag_uri; - - ASSERT_EQ(ret_service_infos.size(), 1); - EXPECT_EQ(ret_service_infos[0]->name, "/test_service"); - EXPECT_EQ(ret_service_infos[0]->type, "test_msgs/srv/BasicTypes"); - EXPECT_EQ(ret_service_infos[0]->request_count, num_service_requests); - EXPECT_EQ(ret_service_infos[0]->response_count, num_service_requests); - EXPECT_EQ(ret_service_infos[0]->serialization_format, "cdr"); + ASSERT_NO_THROW( + ret_service_action_infos = info.read_service_and_action_info( + recorded_bag_uri, storage_id)) << recorded_bag_uri; + + ASSERT_EQ(ret_service_action_infos.first.size(), 1); + EXPECT_EQ(ret_service_action_infos.first[0]->name, "/test_service"); + EXPECT_EQ(ret_service_action_infos.first[0]->type, "test_msgs/srv/BasicTypes"); + EXPECT_EQ(ret_service_action_infos.first[0]->request_count, num_service_requests); + EXPECT_EQ(ret_service_action_infos.first[0]->response_count, num_service_requests); + EXPECT_EQ(ret_service_action_infos.first[0]->serialization_format, "cdr"); } TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_and_services) { @@ -284,7 +290,7 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_and_se storage_options.storage_id = storage_id; storage_options.uri = bag_path_str; rosbag2_transport::RecordOptions record_options = - {true, true, false, {}, {}, {}, {"/rosout"}, {}, {}, "cdr", 100ms}; + {true, true, false, false, {}, {}, {}, {}, {"/rosout"}, {}, {}, {}, "cdr", 100ms}; auto recorder = std::make_shared( std::move(writer), storage_options, record_options); recorder->record(); @@ -330,19 +336,22 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_and_se } rosbag2_cpp::Info info; - std::vector> ret_service_infos; + std::pair>, + std::vector>> + ret_service_action_infos; const std::string recorded_bag_uri = bag_path_str + "/" + get_bag_file_name(); - ASSERT_NO_THROW(ret_service_infos = info.read_service_info(recorded_bag_uri, storage_id)) << - recorded_bag_uri; - ASSERT_EQ(ret_service_infos.size(), 2); - if (ret_service_infos[0]->name == "/test_service2") { - EXPECT_EQ(ret_service_infos[1]->name, "/test_service1"); + ASSERT_NO_THROW( + ret_service_action_infos = info.read_service_and_action_info( + recorded_bag_uri, storage_id)) << recorded_bag_uri; + ASSERT_EQ(ret_service_action_infos.first.size(), 2); + if (ret_service_action_infos.first[0]->name == "/test_service2") { + EXPECT_EQ(ret_service_action_infos.first[1]->name, "/test_service1"); } else { - EXPECT_EQ(ret_service_infos[0]->name, "/test_service1"); - EXPECT_EQ(ret_service_infos[1]->name, "/test_service2"); + EXPECT_EQ(ret_service_action_infos.first[0]->name, "/test_service1"); + EXPECT_EQ(ret_service_action_infos.first[1]->name, "/test_service2"); } - for (const auto & service_info : ret_service_infos) { + for (const auto & service_info : ret_service_action_infos.first) { EXPECT_EQ(service_info->request_count, num_service_requests); EXPECT_EQ(service_info->response_count, num_service_requests); EXPECT_EQ(service_info->type, "test_msgs/srv/BasicTypes"); diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index 50457ac10e..c7bfc2558b 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -72,13 +73,13 @@ TEST_P(InfoEndToEndTestFixture, info_end_to_end_test) { TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_and_topic_name_option) { internal::CaptureStdout(); auto exit_code = execute_and_wait_until_completion( - "ros2 bag info bag_with_topics_and_service_events --verbose --topic-name", + "ros2 bag info bag_with_topics_and_service_events_and_action --verbose --topic-name", bags_path_); std::string output = internal::GetCapturedStdout(); auto expected_storage = GetParam(); auto expected_file = rosbag2_test_common::bag_filename_for_storage_id( - "bag_with_topics_and_service_events", GetParam()); - std::string expected_ros_distro = "unknown"; + "bag_with_topics_and_service_events_and_action", GetParam()); + std::string expected_ros_distro = "rolling"; EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); @@ -91,66 +92,88 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_and_topic_name_option) EXPECT_THAT( output, ContainsRegex( "\nFiles: " + expected_file + - "\nBag size: .*B" + "\nBag size: .* KiB" "\nStorage id: " + expected_storage + "\nROS Distro: " + expected_ros_distro + - "\nDuration: 0\\.0706.*s" - "\nStart: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" - "\nEnd: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" - "\nMessages: 2" + "\nDuration: .*s" + "\nStart: Mar 14 2025 .*:.*:.*\\..* \\(.*\\..*\\)" + "\nEnd: Mar 14 2025 .*:.*:.*\\..* \\(.*\\..*\\)" + "\nMessages: .*" "\nTopic information: ")); - EXPECT_THAT(output, HasSubstr("Service: 2\n")); + EXPECT_THAT(output, HasSubstr("Services: 2\n")); } TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { internal::CaptureStdout(); auto exit_code = execute_and_wait_until_completion( - "ros2 bag info bag_with_topics_and_service_events --verbose", + "ros2 bag info bag_with_topics_and_service_events_and_action --verbose", bags_path_); std::string output = internal::GetCapturedStdout(); auto expected_storage = GetParam(); auto expected_file = rosbag2_test_common::bag_filename_for_storage_id( - "bag_with_topics_and_service_events", GetParam()); - std::string expected_ros_distro = "unknown"; + "bag_with_topics_and_service_events_and_action", GetParam()); + std::string expected_ros_distro = "rolling"; EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); // The bag size depends on the os and is not asserted, the time is asserted time zone independent EXPECT_THAT( output, ContainsRegex( "\nFiles: " + expected_file + - "\nBag size: .*B" + "\nBag size: .* KiB" "\nStorage id: " + expected_storage + "\nROS Distro: " + expected_ros_distro + - "\nDuration: 0\\.0706.*s" - "\nStart: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" - "\nEnd: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" - "\nMessages: 2" + "\nDuration: .*s" + "\nStart: Mar 14 2025 .*:.*:.*\\..* \\(.*\\..*\\)" + "\nEnd: Mar 14 2025 .*:.*:.*\\..* \\(.*\\..*\\)" + "\nMessages: .*" "\nTopic information: ")); - EXPECT_THAT( - output, HasSubstr( - "Topic: /events/write_split | Type: rosbag2_interfaces/msg/WriteSplitEvent | Count: 0 | " - "Size Contribution: 0 B | Serialization Format: cdr\n")); - EXPECT_THAT( - output, HasSubstr( - "Topic: /test_topic1 | Type: test_msgs/msg/Strings | Count: 1 | " - "Size Contribution: 217 B | Serialization Format: cdr\n")); - EXPECT_THAT( - output, HasSubstr( - "Topic: /test_topic2 | Type: test_msgs/msg/Strings | Count: 1 | " - "Size Contribution: 217 B | Serialization Format: cdr\n")); - - EXPECT_THAT(output, HasSubstr("Service: 2\n")); - - EXPECT_THAT( - output, HasSubstr( - "Service: /test_service1 | Type: test_msgs/srv/BasicTypes | Request Count: 2 | " - "Response Count: 2 | Size Contribution: 434 B | Serialization Format: cdr\n")); - EXPECT_THAT( - output, HasSubstr( - "Service: /test_service2 | Type: test_msgs/srv/BasicTypes | Request Count: 2 | " - "Response Count: 2 | Size Contribution: 434 B | Serialization Format: cdr\n")); + // On the Windows platform, ContainsRegex cannot correctly handle the "|" character, + // so replace it with std::regex to check. + std::regex topic_1_info_pattern( + R"(Topic: /test_topic1 | Type: std_msgs/msg/String | Count: \d+" + " | Size Contribution: \d+ B | Serialization Format: cdr\n)"); + EXPECT_TRUE(std::regex_search(output, topic_1_info_pattern)); + + std::regex topic_2_info_pattern( + R"(Topic: /test_topic2 | Type: std_msgs/msg/String | Count: \d+" + " | Size Contribution: \\+d B \\| Serialization Format: cdr\n)"); + EXPECT_TRUE(std::regex_search(output, topic_2_info_pattern)); + + EXPECT_THAT(output, HasSubstr("Services: 2\n")); + + std::regex service_1_info_pattern( + R"(Service: /test_service1 | Type: example_interfaces/srv/AddTwoInts | Request Count: \d+ " + "| Response Count: \d+ | Size Contribution: \d+ B | Serialization Format: cdr\n)"); + EXPECT_TRUE(std::regex_search(output, service_1_info_pattern)); + + std::regex service_2_info_pattern( + R"(Service: /test_service2 | Type: example_interfaces/srv/AddTwoInts | Request Count: \d+ " + "| Response Count: \d+ | Size Contribution: \d+ B | Serialization Format: cdr\n)"); + EXPECT_TRUE(std::regex_search(output, service_2_info_pattern)); + + EXPECT_THAT(output, HasSubstr("Actions: 2\nAction information:")); + + std::regex action_1_info_pattern( + R"(Action: /test_action1 | Type: example_interfaces/action/Fibonacci | Topics: 2 | Service: 3" + " | Size Contribution: \d+ B | Serialization Format: cdr\n" + " Topic: feedback | Count: \d+\n" + " Topic: status | Count: \d+\n" + " Service: send_goal | Request Count: \d+ | Response Count: \d+\n" + " Service: cancel_goal | Request Count: \d+ | Response Count: \d+\n" + " Service: get_result | Request Count: \d+ | Response Count: \d+)"); + EXPECT_TRUE(std::regex_search(output, action_1_info_pattern)); + + std::regex action_2_info_pattern( + R"(Action: /test_action2 | Type: example_interfaces/action/Fibonacci | Topics: 2 | Service: 3" + " | Size Contribution: \d+ B | Serialization Format: cdr\n" + " Topic: feedback | Count: \d+\n" + " Topic: status | Count: \d+\n" + " Service: send_goal | Request Count: \d+ | Response Count: \d+\n" + " Service: cancel_goal | Request Count: \d+ | Response Count: \d+\n" + " Service: get_result | Request Count: \d+ | Response Count: \d+)"); + EXPECT_TRUE(std::regex_search(output, action_2_info_pattern)); } TEST_P(InfoEndToEndTestFixture, info_basic_types_and_arrays_with_verbose_option_end_to_end_test) { diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp index 3126e41884..4258d0d51f 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp @@ -532,7 +532,7 @@ TEST_P(RecordFixture, record_fails_gracefully_if_bag_already_exists) { EXPECT_THAT(error_output, HasSubstr("Output folder 'empty_dir' already exists")); } -TEST_P(RecordFixture, record_if_topic_list_service_list_and_all_are_specified) { +TEST_P(RecordFixture, record_if_topic_list_service_list_action_list_and_all_are_specified) { auto message = get_messages_strings()[0]; message->string_value = "test"; @@ -542,7 +542,8 @@ TEST_P(RecordFixture, record_if_topic_list_service_list_and_all_are_specified) { internal::CaptureStdout(); auto process_handle = start_execution( get_base_record_command() + - " -a --all-topics --all-services --topics /test_topic --services /service1"); + " -a --all-topics --all-services --all-actions --topics /test_topic --services /service1 " + "--actions /action1"); auto cleanup_process_handle = rcpputils::make_scope_exit( [process_handle]() { stop_execution(process_handle); @@ -556,6 +557,7 @@ TEST_P(RecordFixture, record_if_topic_list_service_list_and_all_are_specified) { EXPECT_THAT(output, HasSubstr("--all or --all-topics will override --topics")); EXPECT_THAT(output, HasSubstr("--all or --all-services will override --services")); + EXPECT_THAT(output, HasSubstr("--all or --all-actions will override --actions")); } TEST_P(RecordFixture, record_fails_if_neither_all_nor_topic_list_are_specified) { diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index 5d6d22bb38..53d52de01e 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -32,13 +32,16 @@ struct RecordOptions public: bool all_topics = false; bool all_services = false; + bool all_actions = false; bool is_discovery_disabled = false; std::vector topics; std::vector topic_types; - std::vector services; // service event topic + std::vector services; // service event topics list + std::vector actions; // actions name list std::vector exclude_topics; std::vector exclude_topic_types; - std::vector exclude_service_events; // service event topic + std::vector exclude_service_events; // service event topics list + std::vector exclude_actions; // actions name list std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; diff --git a/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp b/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp index 4335fbcd93..1c7e00bef8 100644 --- a/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp +++ b/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp @@ -62,6 +62,14 @@ class ROSBAG2_TRANSPORT_PUBLIC TopicFilter bool allow_unknown_types_ = false; std::unordered_set already_warned_unknown_types_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + + /// The action name in record_options.include_action will be converted into the action interface + /// name and saved in this set + std::unordered_set include_action_interface_names_; + + /// The action name in record_options.exclude_action will be converted into the action interface + /// name and saved in this set + std::unordered_set exclude_action_interface_names_; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index 388365588b..04e4da693d 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -16,6 +16,7 @@ #include #include "rclcpp/logging.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/play_options.hpp" @@ -267,6 +268,7 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) RecordOptions record_options{}; record_options.all_topics = node.declare_parameter("record.all_topics", false); record_options.all_services = node.declare_parameter("record.all_services", false); + record_options.all_actions = node.declare_parameter("record.all_actions", false); record_options.is_discovery_disabled = node.declare_parameter("record.is_discovery_disabled", false); @@ -285,6 +287,9 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) } record_options.services = service_list; + record_options.actions = node.declare_parameter>( + "record.actions", std::vector()); + record_options.exclude_topics = node.declare_parameter>( "record.exclude_topics", std::vector()); @@ -299,6 +304,9 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) } record_options.exclude_service_events = exclude_service_list; + record_options.exclude_actions = node.declare_parameter>( + "record.exclude_actions", std::vector()); + record_options.rmw_serialization_format = node.declare_parameter("record.rmw_serialization_format", "cdr"); diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index 127d0b8312..11a3b256f3 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -29,17 +29,20 @@ Node convert::encode( Node node; node["all_topics"] = record_options.all_topics; node["all_services"] = record_options.all_services; + node["all_actions"] = record_options.all_actions; node["is_discovery_disabled"] = record_options.is_discovery_disabled; node["topics"] = record_options.topics; node["topic_types"] = record_options.topic_types; node["exclude_topic_types"] = record_options.exclude_topic_types; node["services"] = record_options.services; + node["actions"] = record_options.actions; node["rmw_serialization_format"] = record_options.rmw_serialization_format; node["topic_polling_interval"] = record_options.topic_polling_interval; node["regex"] = record_options.regex; node["exclude_regex"] = record_options.exclude_regex; node["exclude_topics"] = record_options.exclude_topics; node["exclude_services"] = record_options.exclude_service_events; + node["exclude_actions"] = record_options.exclude_actions; node["node_prefix"] = record_options.node_prefix; node["compression_mode"] = record_options.compression_mode; node["compression_format"] = record_options.compression_format; @@ -60,17 +63,20 @@ bool convert::decode( { optional_assign(node, "all_topics", record_options.all_topics); optional_assign(node, "all_services", record_options.all_services); + optional_assign(node, "all_actions", record_options.all_actions); bool record_options_all{false}; // Parse `all` for backward compatability and convenient usage optional_assign(node, "all", record_options_all); if (record_options_all) { record_options.all_topics = true; record_options.all_services = true; + record_options.all_actions = true; } optional_assign(node, "is_discovery_disabled", record_options.is_discovery_disabled); optional_assign>(node, "topics", record_options.topics); optional_assign>(node, "topic_types", record_options.topic_types); optional_assign>(node, "services", record_options.services); + optional_assign>(node, "actions", record_options.actions); optional_assign( node, "rmw_serialization_format", record_options.rmw_serialization_format); @@ -87,6 +93,8 @@ bool convert::decode( record_options.exclude_topic_types); optional_assign>( node, "exclude_services", record_options.exclude_service_events); + optional_assign>( + node, "exclude_actions", record_options.exclude_actions); optional_assign(node, "node_prefix", record_options.node_prefix); optional_assign(node, "compression_mode", record_options.compression_mode); optional_assign(node, "compression_format", record_options.compression_format); diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index c2b767efe2..52489320d1 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -22,8 +22,9 @@ #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rcpputils/split.hpp" -#include "rosbag2_cpp/typesupport_helpers.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/service_utils.hpp" +#include "rosbag2_cpp/typesupport_helpers.hpp" #include "logging.hpp" #include "rosbag2_transport/topic_filter.hpp" @@ -103,7 +104,25 @@ TopicFilter::TopicFilter( : record_options_(std::move(record_options)), allow_unknown_types_(allow_unknown_types), node_graph_(node_graph) -{} +{ + if (record_options_.actions.size() > 0) { + for ( auto & action_name : record_options_.actions ) { + auto action_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(action_name); + include_action_interface_names_.insert( + action_interface_names.begin(), action_interface_names.end()); + } + } + + if (record_options_.exclude_actions.size() > 0) { + for ( auto & action_name : record_options_.exclude_actions ) { + auto action_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(action_name); + exclude_action_interface_names_.insert( + action_interface_names.begin(), action_interface_names.end()); + } + } +} TopicFilter::~TopicFilter() = default; @@ -127,10 +146,15 @@ bool TopicFilter::take_topic( } const std::string & topic_type = topic_types[0]; - bool is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic_name, topic_type); + bool is_action_topic = rosbag2_cpp::is_topic_belong_to_action(topic_name, topic_type); + bool is_service_event_topic = false; + if (!is_action_topic) { + is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic_name, topic_type); + } - if (!is_service_event_topic) { + if (!is_service_event_topic && !is_action_topic) { + // Regular topic if (!record_options_.all_topics && record_options_.topics.empty() && record_options_.topic_types.empty() && @@ -180,7 +204,8 @@ bool TopicFilter::take_topic( "Hidden topics are not recorded. Enable them with --include-hidden-topics"); return false; } - } else { + } else if (is_service_event_topic) { + // Service event topic if (!record_options_.all_services && record_options_.services.empty() && record_options_.regex.empty()) @@ -218,6 +243,49 @@ bool TopicFilter::take_topic( return false; } } + } else if (is_action_topic) { + // action topic + + // Check if topics for action need to be recorded + if (!record_options_.all_actions && + record_options_.actions.empty() && + record_options_.regex.empty()) + { + return false; + } + + // Convert topic name to action name + auto action_name = rosbag2_cpp::action_interface_name_to_action_name(topic_name); + + if (!record_options_.all_actions) { + // Not in include action interface list + if (include_action_interface_names_.find(topic_name) == + include_action_interface_names_.end()) + { + // Not match include regex + if (!record_options_.regex.empty()) { + std::regex include_regex(record_options_.regex); + if (!std::regex_search(action_name, include_regex)) { + return false; + } + } else { + return false; + } + } + } + + if (exclude_action_interface_names_.find(topic_name) != + exclude_action_interface_names_.end()) + { + return false; + } + + if (!record_options_.exclude_regex.empty()) { + std::regex exclude_regex(record_options_.exclude_regex); + if (std::regex_search(action_name, exclude_regex)) { + return false; + } + } } if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) { diff --git a/rosbag2_transport/test/resources/recorder_node_params.yaml b/rosbag2_transport/test/resources/recorder_node_params.yaml index 701f623bf4..89663fddb1 100644 --- a/rosbag2_transport/test/resources/recorder_node_params.yaml +++ b/rosbag2_transport/test/resources/recorder_node_params.yaml @@ -6,11 +6,13 @@ recorder_params_node: record: all_topics: true all_services: true + all_actions: true is_discovery_disabled: true topics: ["topic", "other_topic"] topic_types: ["std_msgs/msg/Header", "geometry_msgs/msg/Pose"] exclude_topic_types: ["sensor_msgs/msg/Image"] services: ["service", "other_service"] + actions: ["action", "other_action"] rmw_serialization_format: "cdr" topic_polling_interval: sec: 0 @@ -19,6 +21,7 @@ recorder_params_node: exclude_regex: "(.*)" exclude_topics: ["exclude_topic", "other_exclude_topic"] exclude_services: ["exclude_service", "other_exclude_service"] + exclude_actions: ["exclude_action", "other_exclude_action"] node_prefix: "prefix" compression_mode: "file" compression_format: "zstd" diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp index 60be757b6a..765f1f0ff3 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -194,7 +194,7 @@ TEST_F(RecordIntegrationTestFixture, test_keyboard_controls) auto keyboard_handler = std::make_shared(); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.start_paused = true; auto recorder = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index a7e3020733..ccd333be0a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -47,7 +47,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, false, {string_topic, array_topic}, {}, {}, {}, {}, {}, "rmw_format", 50ms}; + {false, false, false, false, {string_topic, array_topic}, + {}, {}, {}, {}, {}, {}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -134,7 +135,7 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) test_topic, basic_type_message, num_messages_to_publish, rclcpp::QoS{rclcpp::KeepAll()}, 50ms); rosbag2_transport::RecordOptions record_options = - {false, false, false, {test_topic}, {}, {}, {}, {}, {}, "rmw_format", 50ms}; + {false, false, false, false, {test_topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -218,7 +219,7 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) pub_manager.setup_publisher(topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, false, {topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -283,7 +284,7 @@ TEST_F(RecordIntegrationTestFixture, records_sensor_data) pub_manager.setup_publisher(topic, string_message, 2, rclcpp::SensorDataQoS()); rosbag2_transport::RecordOptions record_options = - {false, false, false, {topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -327,7 +328,7 @@ TEST_F(RecordIntegrationTestFixture, receives_latched_messages) pub_manager.run_publishers(); rosbag2_transport::RecordOptions record_options = - {false, false, false, {topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -373,7 +374,7 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) { topic, profile_transient_local); rosbag2_transport::RecordOptions record_options = - {false, false, false, {topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -422,7 +423,7 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe auto publisher_liveliness = create_pub(profile_liveliness); rosbag2_transport::RecordOptions record_options = - {false, false, false, {topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -462,7 +463,7 @@ TEST_F(RecordIntegrationTestFixture, write_split_callback_is_called) mock_writer.set_max_messages_per_file(5); rosbag2_transport::RecordOptions record_options = - {false, false, false, {string_topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {string_topic}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 2b2b85a40f..4fe05685e1 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -23,10 +23,12 @@ #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" #include "test_msgs/srv/basic_types.hpp" +#include "test_msgs/action/fibonacci.hpp" +#include "rosbag2_test_common/action_client_manager.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" -#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_transport/recorder.hpp" @@ -51,7 +53,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {"/rosout"}, {}, {}, "rmw_format", 100ms}; + {true, false, false, false, {}, {}, {}, {}, {"/rosout", "/events/write_split"}, + {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -99,7 +102,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a "test_service_2"); rosbag2_transport::RecordOptions record_options = - {false, true, false, {}, {}, {}, {"/rosout"}, {}, {}, "rmw_format", 100ms}; + {false, true, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -136,12 +139,62 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a EXPECT_EQ(recorded_messages.size(), expected_messages); } -TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_are_recorded) +TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_actions_are_recorded) +{ + auto action_client_manager_1 = + std::make_shared>( + "test_action_1"); + + auto action_client_manager_2 = + std::make_shared>( + "test_action_2"); + + rosbag2_transport::RecordOptions record_options = + {false, false, true, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + auto cleanup_process_handle = rcpputils::make_scope_exit([&]() {stop_spinning();}); + + ASSERT_TRUE(action_client_manager_1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_client_manager_2->wait_for_action_server_to_be_ready()); + + // Ensure that the introspection service event topic already exists. + ASSERT_TRUE(recorder->wait_for_topic_to_be_discovered( + "/test_action_1/_action/get_result/_service_event")); + ASSERT_TRUE(recorder->wait_for_topic_to_be_discovered( + "/test_action_2/_action/get_result/_service_event")); + + ASSERT_TRUE(action_client_manager_1->send_goal()); + ASSERT_TRUE(action_client_manager_2->send_goal()); + + auto & writer = recorder->get_writer_handle(); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); + + // For one action, 2 send_goal msgs, 2 feedback msgs, 2 status msgs, 2 get_result msgs + constexpr size_t expected_messages = 16; + auto ret = rosbag2_test_common::wait_until_condition( + [ =, &mock_writer]() { + return mock_writer.get_messages().size() >= expected_messages; + }, + std::chrono::seconds(5)); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_EQ(recorded_messages.size(), expected_messages); +} + +TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_service_action_are_recorded) { auto client_manager_1 = std::make_shared>( "test_service"); + auto action_manager_1 = + std::make_shared>( + "test_action_1"); + auto string_message = get_messages_strings()[0]; string_message->string_value = "Hello World"; std::string string_topic = "/string_topic"; @@ -149,7 +202,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a pub_manager.setup_publisher(string_topic, string_message, 1); rosbag2_transport::RecordOptions record_options = - {true, true, false, {}, {}, {}, {"/rosout"}, {}, {}, "rmw_format", 100ms}; + {true, true, true, false, {}, {}, {}, {}, {"/rosout", "/events/write_split"}, + {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -161,6 +215,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a ASSERT_TRUE(client_manager_1->wait_for_service_to_be_ready()); + ASSERT_TRUE(action_manager_1->wait_for_action_server_to_be_ready()); + // At this point, we expect that the service /test_service_1, along with the topic /string_topic, // along with the event topic /test_service_1, along with the split topic /events/write_split are // available to be recorded. However, wait_for_matched() and wait_for_service_to_be_ready() only @@ -168,16 +224,23 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a // make sure it has successfully subscribed to all. ASSERT_TRUE(recorder->wait_for_topic_to_be_discovered("/test_service/_service_event")); + // Ensure that the introspection service event topic already exists. + ASSERT_TRUE(recorder->wait_for_topic_to_be_discovered( + "/test_action_1/_action/get_result/_service_event")); + pub_manager.run_publishers(); // By default, only client introspection is enabled. // For one request, service event topic get 2 messages. ASSERT_TRUE(client_manager_1->send_request()); + // For one action, 2 send_goal msgs, 2 feedback msgs, 2 status msgs, 2 get_result msgs + ASSERT_TRUE(action_manager_1->send_goal()); + auto & writer = recorder->get_writer_handle(); auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); - constexpr size_t expected_messages = 3; + constexpr size_t expected_messages = 1 + 2 + 8; auto ret = rosbag2_test_common::wait_until_condition( [ =, &mock_writer]() { return mock_writer.get_messages().size() >= expected_messages; @@ -187,3 +250,52 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a auto recorded_messages = mock_writer.get_messages(); EXPECT_EQ(recorded_messages.size(), expected_messages); } + +TEST_F(RecordIntegrationTestFixture, cancel_event_messages_from_action_are_recorded) +{ + auto action_manager_1 = + std::make_shared>( + "test_action_1", 2s); + + rosbag2_transport::RecordOptions record_options = + {false, false, true, false, {}, {}, {}, {}, {"/rosout", "/events/write_split"}, + {}, {}, {}, "rmw_format", 100ms}; + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + auto cleanup_process_handle = rcpputils::make_scope_exit([&]() {stop_spinning();}); + + ASSERT_TRUE(action_manager_1->wait_for_action_server_to_be_ready()); + + // Ensure that the introspection service event topic already exists. + ASSERT_TRUE(recorder->wait_for_topic_to_be_discovered( + "/test_action_1/_action/cancel_goal/_service_event")); + + ASSERT_TRUE(action_manager_1->send_goal(true)); + + auto & writer = recorder->get_writer_handle(); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); + + // 2 send_goal msgs, 2 cancel goal msgs , 2 status msgs, 2 get_result msgs + // feedback msgs (Not sure) + constexpr size_t expected_messages = 8; + auto ret = rosbag2_test_common::wait_until_condition( + [ =, &mock_writer]() { + return mock_writer.get_messages().size() > expected_messages; + }, + std::chrono::seconds(5)); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_GT(recorded_messages.size(), expected_messages); + + // Confirm cancel goal messages are recorded + size_t cancel_goal_msg_count = 0; + for (auto & msg : recorded_messages) { + if (msg->topic_name == "/test_action_1/_action/cancel_goal/_service_event") { + cancel_goal_msg_count++; + } + } + EXPECT_EQ(cancel_goal_msg_count, 2); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp index 17080477a6..2973023a3e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp @@ -52,7 +52,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_two_topics_ignore_l pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.ignore_leaf_topics = true; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp index d0a07d33ef..a55b82c9d9 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp @@ -36,7 +36,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_false_ignore string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -55,7 +55,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_true_include string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = true; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -76,7 +76,7 @@ TEST_F( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp index ce9181514d..fe1bd26960 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp @@ -38,7 +38,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_without_discovery_ignores_later_ string_message->string_value = "Hello World"; rosbag2_transport::RecordOptions record_options = - {true, false, true, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, true, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp index 2e612310b1..f2b626ef22 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp @@ -91,7 +91,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_with_sim_time) rosbag2_transport::RecordOptions record_options = { - false, false, false, {string_topic, clock_topic}, {}, {}, {}, {}, {}, "rmw_format", 100ms + false, false, false, false, {string_topic, clock_topic}, {}, {}, {}, {}, {}, {}, {}, + "rmw_format", 100ms }; record_options.use_sim_time = true; auto recorder = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp index 9aa5e4d79e..270839e325 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -23,11 +23,14 @@ TEST(record_options, test_yaml_serialization) rosbag2_transport::RecordOptions original; original.all_topics = true; original.all_services = true; + original.all_actions = true; original.is_discovery_disabled = true; original.topics = {"topic", "other_topic"}; original.services = {"service", "other_service"}; + original.actions = {"action", "other_action"}; original.exclude_topics = {"exclude_topic1", "exclude_topic2"}; original.exclude_service_events = {"exclude_service1", "exclude_service2"}; + original.exclude_actions = {"exclude_action1", "exclude_action2"}; original.rmw_serialization_format = "cdr"; original.topic_polling_interval = std::chrono::milliseconds{200}; original.regex = "[xyz]/topic"; @@ -51,11 +54,14 @@ TEST(record_options, test_yaml_serialization) #define CHECK(field) ASSERT_EQ(original.field, reconstructed.field) CHECK(all_topics); CHECK(all_services); + CHECK(all_actions); CHECK(is_discovery_disabled); CHECK(topics); CHECK(services); + CHECK(actions); CHECK(exclude_topics); CHECK(exclude_service_events); + CHECK(exclude_actions); CHECK(rmw_serialization_format); #undef CHECK } @@ -74,6 +80,7 @@ TEST(record_options, test_yaml_decode_for_all_and_exclude) auto record_options = loaded_node.as(); ASSERT_EQ(record_options.all_topics, true); ASSERT_EQ(record_options.all_services, true); + ASSERT_EQ(record_options.all_actions, true); ASSERT_EQ(record_options.regex, "[xyz]/topic"); ASSERT_EQ(record_options.exclude_regex, "[x]/topic"); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 6cf26a4bed..ea3296be57 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -22,12 +22,15 @@ #include "rclcpp/rclcpp.hpp" +#include "rosbag2_test_common/action_client_manager.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" -#include "rosbag2_test_common/client_manager.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_transport/recorder.hpp" +#include "test_msgs/action/fibonacci.hpp" #include "test_msgs/msg/arrays.hpp" #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" @@ -62,7 +65,7 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) ASSERT_FALSE(std::regex_match(b4, re)); rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; // TODO(karsten1987) Refactor this into publication manager @@ -135,7 +138,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_topic_recording) ASSERT_TRUE(std::regex_match(e1, exclude)); rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; record_options.exclude_regex = topics_regex_to_exclude; @@ -211,7 +214,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_topic_recording) ASSERT_TRUE(e1 == topics_exclude); rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; record_options.exclude_topics.emplace_back(topics_exclude); @@ -274,7 +277,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_service_recording) std::string b2 = "/namespace_before/not_nice"; rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; record_options.exclude_regex = services_regex_to_exclude; @@ -356,7 +359,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording std::string b2 = "/namespace_before/not_nice"; rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; record_options.exclude_service_events.emplace_back(services_exclude); @@ -420,3 +423,201 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording EXPECT_TRUE(recorded_topics.find(v1 + "/_service_event") != recorded_topics.end()); EXPECT_TRUE(recorded_topics.find(v2 + "/_service_event") != recorded_topics.end()); } + +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_action_recording) +{ + std::string regex = "/[a-z]+_nice(_.*)"; + std::string actions_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; + + // matching action + std::string v1 = "/awesome_nice_action"; + std::string v2 = "/still_nice_action"; + + // excluded action + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // action that shouldn't match + std::string b1 = "/numberslike1arenot_nice"; + std::string b2 = "/namespace_before/not_nice"; + + rosbag2_transport::RecordOptions record_options = + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + record_options.regex = regex; + record_options.exclude_regex = actions_regex_to_exclude; + + auto action_manager_v1 = + std::make_shared>(v1); + + auto action_manager_v2 = + std::make_shared>(v2); + + auto action_manager_e1 = + std::make_shared>(e1); + + auto action_manager_b1 = + std::make_shared>(b1); + + auto action_manager_b2 = + std::make_shared>(b2); + + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + auto cleanup_process_handle = rcpputils::make_scope_exit([&]() {stop_spinning();}); + + ASSERT_TRUE(action_manager_v1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_v2->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_e1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_b1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_b2->wait_for_action_server_to_be_ready()); + + // Ensure that the introspection service event topic already exists. + ASSERT_TRUE( + recorder->wait_for_topic_to_be_discovered(v1 + "/_action/get_result/_service_event")); + ASSERT_TRUE( + recorder->wait_for_topic_to_be_discovered(v2 + "/_action/get_result/_service_event")); + + auto & writer = recorder->get_writer_handle(); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); + + ASSERT_TRUE(action_manager_v1->send_goal()); + ASSERT_TRUE(action_manager_v2->send_goal()); + ASSERT_TRUE(action_manager_e1->send_goal()); + ASSERT_TRUE(action_manager_b1->send_goal()); + ASSERT_TRUE(action_manager_b2->send_goal()); + + // One action include at least 8 messages + // Goal request received, Goal response sent, 2 feedback, Result request received + // Result response sent, 2 status + constexpr size_t expected_at_least_messages_size = 16; + auto ret = rosbag2_test_common::wait_until_condition( + [ =, &mock_writer]() { + return mock_writer.get_messages().size() >= expected_at_least_messages_size; + }, + std::chrono::seconds(5)); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_THAT(recorded_messages, SizeIs(expected_at_least_messages_size)); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_EQ(recorded_topics.size(), 10); // One action is related to 5 topics + + std::vector expected_action_interface_names; + auto expected_action_v1_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(v1); + auto expected_action_v2_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(v2); + std::copy(expected_action_v1_interface_names.begin(), + expected_action_v1_interface_names.end(), + std::back_inserter(expected_action_interface_names)); + std::copy(expected_action_v2_interface_names.begin(), + expected_action_v2_interface_names.end(), + std::back_inserter(expected_action_interface_names)); + + for (const auto & topic : expected_action_interface_names) { + EXPECT_TRUE(recorded_topics.find(topic) != recorded_topics.end()) << + "Expected topic:" << topic; + } +} + +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_actions_action_recording) +{ + std::string regex = "/[a-z]+_nice(_.*)"; + std::vector action_exclude = { + "/quite_nice_namespace/but_it_is_excluded", + }; + + // matching actions + std::string v1 = "/awesome_nice_action"; + std::string v2 = "/still_nice_action"; + + // excluded action + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // action that shouldn't match + std::string b1 = "/numberslike1arenot_nice"; + std::string b2 = "/namespace_before/not_nice"; + + rosbag2_transport::RecordOptions record_options = + {false, false, false, false, {}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 10ms}; + record_options.regex = regex; + record_options.exclude_actions = action_exclude; + + auto action_manager_v1 = + std::make_shared>(v1); + + auto action_manager_v2 = + std::make_shared>(v2); + + auto action_manager_e1 = + std::make_shared>(e1); + + auto action_manager_b1 = + std::make_shared>(b1); + + auto action_manager_b2 = + std::make_shared>(b2); + + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + auto cleanup_process_handle = rcpputils::make_scope_exit([&]() {stop_spinning();}); + + ASSERT_TRUE(action_manager_v1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_v2->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_e1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_b1->wait_for_action_server_to_be_ready()); + ASSERT_TRUE(action_manager_b2->wait_for_action_server_to_be_ready()); + + // Ensure that the introspection service event topic already exists. + ASSERT_TRUE( + recorder->wait_for_topic_to_be_discovered(v1 + "/_action/get_result/_service_event")); + ASSERT_TRUE( + recorder->wait_for_topic_to_be_discovered(v2 + "/_action/get_result/_service_event")); + + auto & writer = recorder->get_writer_handle(); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); + + ASSERT_TRUE(action_manager_v1->send_goal()); + ASSERT_TRUE(action_manager_v2->send_goal()); + ASSERT_TRUE(action_manager_e1->send_goal()); + ASSERT_TRUE(action_manager_b1->send_goal()); + ASSERT_TRUE(action_manager_b2->send_goal()); + + // One action include at least 8 messages + // Goal request received, Goal response sent, 2 feedback, Result request received + // Result response sent, 2 status + constexpr size_t expected_at_least_messages_size = 16; + auto ret = rosbag2_test_common::wait_until_condition( + [ =, &mock_writer]() { + return mock_writer.get_messages().size() >= expected_at_least_messages_size; + }, + std::chrono::seconds(5)); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_THAT(recorded_messages, SizeIs(expected_at_least_messages_size)); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_EQ(recorded_topics.size(), 10); // One action is related to 5 topics + + std::vector expected_action_interface_names; + auto expected_action_v1_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(v1); + auto expected_action_v2_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(v2); + std::copy(expected_action_v1_interface_names.begin(), + expected_action_v1_interface_names.end(), + std::back_inserter(expected_action_interface_names)); + std::copy(expected_action_v2_interface_names.begin(), + expected_action_v2_interface_names.end(), + std::back_inserter(expected_action_interface_names)); + + for (const auto & topic : expected_action_interface_names) { + EXPECT_TRUE(recorded_topics.find(topic) != recorded_topics.end()) << + "Expected topic:" << topic; + } +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index f585549bca..adf55dc039 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp @@ -68,7 +68,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture client_node_ = std::make_shared("test_record_client"); rosbag2_transport::RecordOptions record_options = - {false, false, false, {test_topic_}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, false, false, false, {test_topic_}, {}, {}, {}, {}, {}, {}, {}, "rmw_format", 100ms}; storage_options_.snapshot_mode = snapshot_mode_; storage_options_.max_cache_size = 200; recorder_ = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index f4191384d4..9cacf70545 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -22,6 +22,7 @@ #include #include +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_transport/topic_filter.hpp" using namespace ::testing; // NOLINT @@ -29,7 +30,7 @@ using namespace ::testing; // NOLINT class TestTopicFilter : public Test { protected: - std::map> topics_and_types_with_services_ = { + std::map> topics_services_actions_with_types_ = { {"/planning1", {"planning_topic_type"}}, {"/planning2", {"planning_topic_type"}}, {"/invalid_topic", {"invalid_topic_type"}}, @@ -37,10 +38,50 @@ class TestTopicFilter : public Test {"/localization", {"localization_topic_type"}}, {"/invisible", {"invisible_topic_type"}}, {"/status", {"status_topic_type"}}, + // For services {"/invalid_service/_service_event", {"service/srv/invalid_service_Event"}}, {"/invalidated_service/_service_event", {"service/srv/invalidated_service_Event"}}, - {"/planning_service/_service_event", {"service/srv/planning_service_Event"}} + {"/planning_service/_service_event", {"service/srv/planning_service_Event"}}, + // invalid_action + {"/invalid_action/_action/send_goal/_service_event", + {"test_msgs/action/Invalid_SendGoal_Event"}}, + {"/invalid_action/_action/get_result/_service_event", + {"test_msgs/action/Invalid_GetResult_Event"}}, + {"/invalid_action/_action/cancel_goal/_service_event", {"action_msgs/srv/CancelGoal_Event"}}, + {"/invalid_action/_action/feedback", {"test_msgs/action/Invalid_FeedbackMessage"}}, + {"/invalid_action/_action/status", {"action_msgs/msg/GoalStatusArray"}}, + // invalidated_action + {"/invalidated_action/_action/send_goal/_service_event", + {"test_msgs/action/Invalidated_SendGoal_Event"}}, + {"/invalidated_action/_action/get_result/_service_event", + {"test_msgs/action/Invalidated_GetResult_Event"}}, + {"/invalidated_action/_action/cancel_goal/_service_event", + {"action_msgs/srv/CancelGoal_Event"}}, + {"/invalidated_action/_action/feedback", {"test_msgs/action/Invalidated_FeedbackMessage"}}, + {"/invalidated_action/_action/status", {"action_msgs/msg/GoalStatusArray"}}, + // planning_action + {"/planning_action/_action/send_goal/_service_event", + {"unknown_pkg/action/Planning_SendGoal_Event"}}, + {"/planning_action/_action/get_result/_service_event", + {"unknown_pkg/action/Planning_GetResult_Event"}}, + {"/planning_action/_action/cancel_goal/_service_event", {"action_msgs/srv/CancelGoal_Event"}}, + {"/planning_action/_action/feedback", {"unknown_pkg/action/Planning_FeedbackMessage"}}, + {"/planning_action/_action/status", {"action_msgs/msg/GoalStatusArray"}}, }; + + void check_action_interfaces_exist( + std::unordered_map & filtered_topics, + const std::string action_name) + { + EXPECT_TRUE(filtered_topics.find(action_name + "/_action/send_goal/_service_event") != + filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find(action_name + "/_action/get_result/_service_event") != + filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find(action_name + "/_action/cancel_goal/_service_event") != + filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find(action_name + "/_action/feedback") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find(action_name + "/_action/status") != filtered_topics.end()); + } }; TEST_F(TestTopicFilter, filter_hidden_topics) { @@ -201,13 +242,86 @@ TEST_F(TestTopicFilter, filter_services) { } } +TEST_F(TestTopicFilter, filter_actions) { + std::map> topics_and_types{ + {"topic/a", {"type_a"}}, + {"/service/a/_service_event", {"service/srv/type_a_Event"}}, + // action/a + {"/action/a/_action/send_goal/_service_event", {"test_msgs/action/TypeA_SendGoal_Event"}}, + {"/action/a/_action/get_result/_service_event", {"test_msgs/action/TypeA_GetResult_Event"}}, + {"/action/a/_action/cancel_goal/_service_event", {"action_msgs/srv/CancelGoal_Event"}}, + {"/action/a/_action/feedback", {"test_msgs/action/TypeA_FeedbackMessage"}}, + {"/action/a/_action/status", {"action_msgs/msg/GoalStatusArray"}}, + // action/b + {"/action/b/_action/send_goal/_service_event", {"test_msgs/action/TypeB_SendGoal_Event"}}, + {"/action/b/_action/get_result/_service_event", {"test_msgs/action/TypeB_GetResult_Event"}}, + {"/action/b/_action/cancel_goal/_service_event", {"action_msgs/srv/CancelGoal_Event"}}, + {"/action/b/_action/feedback", {"test_msgs/action/TypeB_FeedbackMessage"}}, + {"/action/b/_action/status", {"action_msgs/msg/GoalStatusArray"}}, + // action/c + {"/action/c/_action/send_goal/_service_event", {"test_msgs/action/TypeC_SendGoal_Event"}}, + {"/action/c/_action/get_result/_service_event", {"test_msgs/action/TypeC_GetResult_Event"}}, + {"/action/c/_action/cancel_goal/_service_event", {"action_msgs/srv/CancelGoal_Event"}}, + {"/action/c/_action/feedback", {"test_msgs/action/TypeC_FeedbackMessage"}}, + {"/action/c/_action/status", {"action_msgs/msg/GoalStatusArray"}}, + }; + + { + rosbag2_transport::RecordOptions record_options; + + record_options.actions = { + "/action/a" + }; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(5u, filtered_topics.size()); + auto expected_action_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(record_options.actions[0]); + for (auto & topic : expected_action_interface_names) { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()) << + "Expected topic:" << topic; + } + } + + { + rosbag2_transport::RecordOptions record_options; + record_options.actions = { + "/action/a", + "/action/b", + "/action/d" + }; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + + // all action interface names for /action/a and /action/b + std::vector expected_action_interfaces_name; + auto expected_action_a_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(record_options.actions[0]); + auto expected_action_b_interface_names = + rosbag2_cpp::action_name_to_action_interface_names(record_options.actions[1]); + std::copy(expected_action_a_interface_names.begin(), + expected_action_a_interface_names.end(), + std::back_inserter(expected_action_interfaces_name)); + std::copy(expected_action_b_interface_names.begin(), + expected_action_b_interface_names.end(), + std::back_inserter(expected_action_interfaces_name)); + + ASSERT_EQ(10u, filtered_topics.size()); + + for (auto & topic : expected_action_interfaces_name) { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()) << + "Expected topic:" << topic; + } + } +} + TEST_F(TestTopicFilter, all_topics_and_exclude_regex) { rosbag2_transport::RecordOptions record_options; record_options.exclude_regex = "/inv.*"; record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(4)); for (const auto & topic : {"/planning1", "/planning2", "/localization", "/status"}) { @@ -224,7 +338,7 @@ TEST_F(TestTopicFilter, all_topics_and_exclude_topics) "/invisible"}; record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(4)); for (const auto & topic : {"/planning1", "/planning2", "/localization", "/status"}) { @@ -240,7 +354,7 @@ TEST_F(TestTopicFilter, all_topics_and_exclude_type_topics) "status_topic_type"}; record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(5)); for (const auto & topic : @@ -257,9 +371,11 @@ TEST_F(TestTopicFilter, all_services_and_exclude_regex) { rosbag2_transport::RecordOptions record_options; record_options.exclude_regex = "/inv.*"; + record_options.all_topics = false; record_options.all_services = true; + record_options.all_actions = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(1)); EXPECT_EQ("/planning_service/_service_event", filtered_topics.begin()->first); @@ -274,7 +390,7 @@ TEST_F(TestTopicFilter, all_services_and_exclude_service_events) }; record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(1)); EXPECT_EQ("/planning_service/_service_event", filtered_topics.begin()->first); @@ -287,7 +403,7 @@ TEST_F(TestTopicFilter, all_topics_all_services_and_exclude_regex) record_options.all_services = true; record_options.exclude_regex = "/inv.*"; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(5)); for (const auto & topic : @@ -301,13 +417,17 @@ TEST_F(TestTopicFilter, regex_and_exclude_regex) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/invalid.*"; - record_options.exclude_regex = ".invalidated.*"; // Only affect topics + record_options.exclude_regex = ".invalidated.*"; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); - EXPECT_THAT(filtered_topics, SizeIs(2)); + EXPECT_THAT(filtered_topics, SizeIs(7)); + // Matched topic EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + // Matched service EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); + // Matched action + check_action_interfaces_exist(filtered_topics, "/invalid_action"); } TEST_F(TestTopicFilter, regex_and_exclude_topics) @@ -316,12 +436,17 @@ TEST_F(TestTopicFilter, regex_and_exclude_topics) record_options.regex = "/invalid.*"; record_options.exclude_topics = {"/invalidated_topic"}; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); - EXPECT_THAT(filtered_topics, SizeIs(3)); + EXPECT_THAT(filtered_topics, SizeIs(13)); + // Matched topic EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + // Matched service EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); EXPECT_TRUE(filtered_topics.find("/invalidated_service/_service_event") != filtered_topics.end()); + // Matched action + check_action_interfaces_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalidated_action"); } TEST_F(TestTopicFilter, regex_and_exclude_service_events) @@ -330,12 +455,38 @@ TEST_F(TestTopicFilter, regex_and_exclude_service_events) record_options.regex = "/invalid.*"; record_options.exclude_service_events = {"/invalidated_service/_service_event"}; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); - EXPECT_THAT(filtered_topics, SizeIs(3)); + EXPECT_THAT(filtered_topics, SizeIs(13)); + // Matched topic + EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalidated_topic") != filtered_topics.end()); + // Matched service + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); + // Matched action + check_action_interfaces_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalidated_action"); +} + +TEST_F(TestTopicFilter, regex_and_exclude_actions) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_actions = { + "/invalidated_action" + }; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); + + EXPECT_THAT(filtered_topics, SizeIs(9)); + // Matched topic EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); EXPECT_TRUE(filtered_topics.find("/invalidated_topic") != filtered_topics.end()); + // Matched service EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalidated_service/_service_event") != filtered_topics.end()); + // Matched action + check_action_interfaces_exist(filtered_topics, "/invalid_action"); } TEST_F(TestTopicFilter, regex_filter) @@ -343,15 +494,21 @@ TEST_F(TestTopicFilter, regex_filter) rosbag2_transport::RecordOptions record_options; record_options.regex = "^/inval"; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); - EXPECT_THAT(filtered_topics, SizeIs(4)); + EXPECT_THAT(filtered_topics, SizeIs(14)); + + // Matched topic and service for (const auto & topic : {"/invalid_topic", "/invalidated_topic", "/invalid_service/_service_event", "/invalidated_service/_service_event"}) { EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()) << "Expected topic:" << topic; } + + // Matched action + check_action_interfaces_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalidated_action"); } TEST_F(TestTopicFilter, all_topics_overrides_regex) @@ -360,7 +517,7 @@ TEST_F(TestTopicFilter, all_topics_overrides_regex) record_options.regex = "/status"; record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(7)); } @@ -370,7 +527,7 @@ TEST_F(TestTopicFilter, topic_types) record_options.topic_types = {{"planning_topic_type"}}; record_options.all_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(2)); EXPECT_TRUE(filtered_topics.find("/planning1") != filtered_topics.end()); EXPECT_TRUE(filtered_topics.find("/planning2") != filtered_topics.end()); @@ -384,7 +541,7 @@ TEST_F(TestTopicFilter, topic_types_topic_names_and_regex) record_options.regex = "^/stat"; record_options.all_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(4)); EXPECT_TRUE(filtered_topics.find("/planning1") != filtered_topics.end()); EXPECT_TRUE(filtered_topics.find("/planning2") != filtered_topics.end()); @@ -400,7 +557,7 @@ TEST_F(TestTopicFilter, topic_types_do_not_overlap_with_services) record_options.all_services = false; record_options.services = {"/invalidated_service/_service_event"}; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(3)); EXPECT_TRUE(filtered_topics.find("/planning1") != filtered_topics.end()); EXPECT_TRUE(filtered_topics.find("/planning2") != filtered_topics.end()); @@ -413,7 +570,7 @@ TEST_F(TestTopicFilter, all_topics_overrides_topic_types) record_options.topic_types = {{"planning_topic_type"}}; record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(7)); } @@ -424,7 +581,7 @@ TEST_F(TestTopicFilter, all_services_overrides_topic_types) record_options.all_topics = false; record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(5)); EXPECT_TRUE(filtered_topics.find("/planning1") != filtered_topics.end()); EXPECT_TRUE(filtered_topics.find("/planning2") != filtered_topics.end()); @@ -437,9 +594,11 @@ TEST_F(TestTopicFilter, do_not_print_warning_about_unknown_types_if_topic_is_not // Select only one topic with name "/planning1" via topic list record_options.topics = {"/planning1"}; record_options.all_topics = false; + record_options.all_services = false; + record_options.all_actions = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, false}; testing::internal::CaptureStderr(); - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); std::string test_output = testing::internal::GetCapturedStderr(); ASSERT_EQ(0u, filtered_topics.size()); EXPECT_TRUE( @@ -455,11 +614,20 @@ TEST_F(TestTopicFilter, do_not_print_warning_about_unknown_types_if_topic_is_not // Select topics wth name starting from "/planning" via regex record_options.regex = "^/planning"; record_options.all_topics = false; + record_options.all_services = false; + record_options.all_actions = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, false}; testing::internal::CaptureStderr(); - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); std::string test_output = testing::internal::GetCapturedStderr(); - ASSERT_EQ(0u, filtered_topics.size()); + + ASSERT_EQ(2u, filtered_topics.size()); + EXPECT_TRUE( + filtered_topics.find("/planning_action/_action/cancel_goal/_service_event") != + filtered_topics.end()); + EXPECT_TRUE( + filtered_topics.find("/planning_action/_action/status") != filtered_topics.end()); + EXPECT_TRUE( test_output.find( "Topic '/invalid_topic' has unknown type 'invalid_topic_type'") == std::string::npos); @@ -470,10 +638,18 @@ TEST_F(TestTopicFilter, do_not_print_warning_about_unknown_types_if_topic_is_not EXPECT_TRUE( test_output.find( "Topic '/planning2' has unknown type 'planning_topic_type'") == std::string::npos); + EXPECT_TRUE( test_output.find( "Topic '/planning_service/_service_event' has unknown type " "'service/srv/planning_service_Event'") != std::string::npos); + + EXPECT_TRUE(test_output.find("'/planning_action/_action/feedback' has unknown type " + "'unknown_pkg/action/Planning_FeedbackMessage'") != std::string::npos); + EXPECT_TRUE(test_output.find("'/planning_action/_action/get_result/_service_event' has " + "unknown type 'unknown_pkg/action/Planning_GetResult_Event'") != std::string::npos); + EXPECT_TRUE(test_output.find("'/planning_action/_action/send_goal/_service_event'" + " has unknown type 'unknown_pkg/action/Planning_SendGoal_Event'") != std::string::npos); } } @@ -484,7 +660,7 @@ TEST_F(TestTopicFilter, all_services_overrides_regex) record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(3)); } @@ -495,6 +671,6 @@ TEST_F(TestTopicFilter, all_topics_and_all_services_overrides_regex) record_options.all_topics = true; record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; - auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + auto filtered_topics = filter.filter_topics(topics_services_actions_with_types_); EXPECT_THAT(filtered_topics, SizeIs(10)); }