From 92b5beb778be45a519ff1d8f888f208a356d98b4 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 14 Mar 2025 20:50:17 +0800 Subject: [PATCH 01/18] Implement action recording and display info about recorded action Signed-off-by: Barry Xu --- ros2bag/ros2bag/api/__init__.py | 16 + ros2bag/ros2bag/verb/info.py | 3 +- ros2bag/ros2bag/verb/record.py | 48 +- ros2bag/test/test_recorder_args_parser.py | 96 +++- rosbag2_cpp/CMakeLists.txt | 13 +- .../include/rosbag2_cpp/action_utils.hpp | 59 +++ rosbag2_cpp/include/rosbag2_cpp/info.hpp | 19 +- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 152 ++++++ rosbag2_cpp/src/rosbag2_cpp/info.cpp | 183 ++++++- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 5 + .../rosbag2_cpp/writers/sequential_writer.cpp | 29 +- .../test/rosbag2_cpp/test_action_utils.cpp | 149 ++++++ rosbag2_py/CMakeLists.txt | 1 + rosbag2_py/src/rosbag2_py/_info.cpp | 66 ++- rosbag2_py/src/rosbag2_py/_transport.cpp | 3 + rosbag2_py/src/rosbag2_py/action_info.hpp | 43 ++ .../src/rosbag2_py/format_action_info.cpp | 71 +++ .../src/rosbag2_py/format_action_info.hpp | 34 ++ .../src/rosbag2_py/format_bag_metadata.cpp | 140 +++++- .../src/rosbag2_py/format_service_info.cpp | 4 +- .../src/rosbag2_py/info_sorting_method.cpp | 83 ++++ .../src/rosbag2_py/info_sorting_method.hpp | 9 + rosbag2_test_common/CMakeLists.txt | 4 +- .../action_client_manager.hpp | 248 ++++++++++ rosbag2_test_common/package.xml | 2 + .../bag_with_topics_and_service_events.mcap | Bin 13271 -> 0 bytes .../metadata.yaml | 57 --- ..._topics_and_service_events_and_action.mcap | Bin 0 -> 76468 bytes .../metadata.yaml | 411 ++++++++++++++++ .../bag_with_topics_and_service_events.db3 | Bin 28672 -> 0 bytes .../metadata.yaml | 57 --- ...h_topics_and_service_events_and_action.db3 | Bin 0 -> 90112 bytes .../metadata.yaml | 456 ++++++++++++++++++ .../test_rosbag2_cpp_get_service_info.cpp | 22 +- .../test_rosbag2_info_end_to_end.cpp | 84 ++-- .../test_rosbag2_record_end_to_end.cpp | 6 +- .../rosbag2_transport/record_options.hpp | 3 + .../config_options_from_node_params.cpp | 20 + .../src/rosbag2_transport/record_options.cpp | 8 + .../src/rosbag2_transport/topic_filter.cpp | 54 ++- .../test/resources/recorder_node_params.yaml | 3 + .../test_keyboard_controls.cpp | 2 +- .../test/rosbag2_transport/test_record.cpp | 17 +- .../rosbag2_transport/test_record_all.cpp | 75 ++- .../test_record_all_ignore_leaf_topics.cpp | 2 +- ..._record_all_include_unpublished_topics.cpp | 6 +- .../test_record_all_no_discovery.cpp | 2 +- .../test_record_all_use_sim_time.cpp | 3 +- .../rosbag2_transport/test_record_options.cpp | 7 + .../rosbag2_transport/test_record_regex.cpp | 184 ++++++- .../test_record_services.cpp | 2 +- .../rosbag2_transport/test_topic_filter.cpp | 209 +++++++- 52 files changed, 2906 insertions(+), 264 deletions(-) create mode 100644 rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp create mode 100644 rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp create mode 100644 rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp create mode 100644 rosbag2_py/src/rosbag2_py/action_info.hpp create mode 100644 rosbag2_py/src/rosbag2_py/format_action_info.cpp create mode 100644 rosbag2_py/src/rosbag2_py/format_action_info.hpp create mode 100644 rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp delete mode 100644 rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap delete mode 100644 rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml create mode 100644 rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.mcap create mode 100644 rosbag2_tests/resources/mcap/bag_with_topics_and_service_events_and_action/metadata.yaml delete mode 100644 rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 delete mode 100644 rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml create mode 100644 rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/bag_with_topics_and_service_events_and_action.db3 create mode 100644 rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events_and_action/metadata.yaml diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index 42af8c9bb3..deafc3ec40 100644 --- a/ros2bag/ros2bag/api/__init__.py +++ b/ros2bag/ros2bag/api/__init__.py @@ -321,3 +321,19 @@ def convert_service_to_service_event_topic(services): services_event_topics.append(service + '/_service_event') return services_event_topics + + +def convert_action_to_all_related_topics(actions): + action_topics = [] + + if not actions: + return action_topics + + for action in actions: + action_topics.append(action + '/_action/send_goal/_service_event') + action_topics.append(action + '/_action/get_result/_service_event') + action_topics.append(action + '/_action/cancel_goal/_service_event') + action_topics.append(action + '/_action/status') + action_topics.append(action + '/_action/feedback') + + return action_topics diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index d101c0c52c..9f78c88aab 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 request/response information for services. Display request/response ' + 'information for action internal services' ) 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..19edb513a7 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -18,6 +18,7 @@ from rclpy.qos import InvalidQoSProfileException from ros2bag.api import add_writer_storage_plugin_extensions +from ros2bag.api import convert_action_to_all_related_topics from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error @@ -70,27 +71,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 +111,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 +229,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 +252,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 +270,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 +302,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 '' + class RecordVerb(VerbExtension): """Record ROS data to a bag.""" @@ -330,11 +353,15 @@ 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) + # Covert action name to internal topic name and internal service event topic name + record_options.actions = convert_action_to_all_related_topics(args.actions) + 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 +371,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 = convert_action_to_all_related_topics(args.exclude_actions) 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..f3089c7caa 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,26 @@ 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.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..6e9c0d7ae6 --- /dev/null +++ b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp @@ -0,0 +1,59 @@ +// 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 +{ +enum class TopicsInAction +{ + SendGoalEvent, + CancelGoalEvent, + GetResultEvent, + Feedback, + Status, + Unknown +}; + +ROSBAG2_CPP_PUBLIC +bool +is_topic_related_to_action(const std::string & topic_name, const std::string & topic_type); + +// Call this function after is_topic_related_to_action() return true +ROSBAG2_CPP_PUBLIC +std::string +action_topic_name_to_action_name(const std::string & topic_name); + +// Call this function after is_topic_related_to_action() return true +// Note that cancel_goal event topic and status topic return "" +ROSBAG2_CPP_PUBLIC +std::string +action_topic_type_to_action_type(const std::string & topic_type); + +ROSBAG2_CPP_PUBLIC +TopicsInAction +get_action_topic_type_from_topic_name(const std::string & topic_name); + +ROSBAG2_CPP_PUBLIC +std::vector +action_name_to_action_topic_name(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..5202bfdc92 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,7 +59,9 @@ 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( + virtual void read_service_action_info( + std::vector> & output_service_info, + std::vector> & output_action_info, const std::string & uri, const std::string & storage_id = ""); virtual std::unordered_map compute_messages_size_contribution( 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..f6ef203709 --- /dev/null +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -0,0 +1,152 @@ +// 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 ActionTopicPostfix = { + {TopicsInAction::SendGoalEvent, "/_action/send_goal/_service_event"}, + {TopicsInAction::CancelGoalEvent, "/_action/cancel_goal/_service_event"}, + {TopicsInAction::GetResultEvent, "/_action/get_result/_service_event"}, + {TopicsInAction::Feedback, "/_action/feedback"}, + {TopicsInAction::Status, "/_action/status"} +}; + +// The regex pattern of the action internal topics and service event topics +const std::unordered_map ActionTopicTypeRegex = { + {TopicsInAction::SendGoalEvent, ".+/action/.+SendGoal_Event$"}, + {TopicsInAction::CancelGoalEvent, "^action_msgs/srv/CancelGoal_Event$"}, + {TopicsInAction::GetResultEvent, ".+/action/.+GetResult_Event$"}, + {TopicsInAction::Feedback, ".+/action/.+_FeedbackMessage$"}, + {TopicsInAction::Status, "^action_msgs/msg/GoalStatusArray$"} +}; + +const size_t kMinActionTopicPostfixLen = ActionTopicPostfix.at(TopicsInAction::Status).length(); + +bool is_topic_related_to_action(const std::string & topic_name, const std::string & topic_type) +{ + TopicsInAction topic = TopicsInAction::Unknown; + if (topic_name.length() <= kMinActionTopicPostfixLen) { + return false; + } else { + for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + if (topic_name.length() > postfix.length() && + topic_name.compare( + topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) + { + topic = topic_type_enum; + break; + } + } + } + + if (topic == TopicsInAction::Unknown) { + return false; + } + + std::regex pattern(ActionTopicTypeRegex.at(topic)); + return std::regex_search(topic_type, pattern); +} + +std::string action_topic_name_to_action_name(const std::string & topic_name) +{ + std::string action_name; + if (topic_name.length() <= kMinActionTopicPostfixLen) { + return action_name; + } else { + for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + 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 action_topic_type_to_action_type(const std::string & topic_type) +{ + std::string service_type; + + for (auto &[topic_type_enum, regex] : ActionTopicTypeRegex) { + std::regex pattern(regex); + if (std::regex_search(topic_type, pattern)) { + switch (topic_type_enum) { + case TopicsInAction::SendGoalEvent: + // Remove the postfix "_SendGoal_Event" + service_type = + topic_type.substr(0, topic_type.length() - std::strlen("_SendGoal_Event")); + break; + case TopicsInAction::GetResultEvent: + // Remove the postfix "_GetResult_Event" + service_type = + topic_type.substr(0, topic_type.length() - std::strlen("_GetResult_Event")); + break; + case TopicsInAction::Feedback: + // Remove the postfix "_FeedbackMessage" + service_type = + topic_type.substr(0, topic_type.length() - std::strlen("_FeedbackMessage")); + break; + case TopicsInAction::CancelGoalEvent: + case TopicsInAction::Status: + default: + break; + } + return service_type; + } + } + + return service_type; +} + +TopicsInAction get_action_topic_type_from_topic_name(const std::string & topic_name) +{ + for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + if (topic_name.length() > postfix.length() && + topic_name.compare( + topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) + { + return topic_type_enum; + } + } + + return TopicsInAction::Unknown; +} + +std::vector action_name_to_action_topic_name(const std::string & action_name) +{ + std::vector action_topics; + + if (action_name.empty()) { + return action_topics; + } + + for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + 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..598cb05b72 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,9 +69,18 @@ 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; +}; } // namespace -std::vector> Info::read_service_info( +void Info::read_service_action_info( + std::vector> & output_service_info, + std::vector> & output_action_info, const std::string & uri, const std::string & storage_id) { rosbag2_storage::StorageFactory factory; @@ -83,14 +94,40 @@ std::vector> Info::read_service_info( throw std::runtime_error("Failed to set read order on " + uri); } + // Service event topic name as key using service_analysis = std::unordered_map>; - - std::unordered_map> all_service_info; service_analysis service_process_info; + std::unordered_map> all_service_info; + + // Action name as key + using action_analysis = + std::unordered_map>; + action_analysis action_process_info; + std::unordered_map> all_action_info; auto all_topics_types = storage->get_all_topics_and_types(); for (auto & t : all_topics_types) { + if (is_topic_related_to_action(t.name, t.type)) { + std::shared_ptr action_info; + std::string action_name = action_topic_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]; + } + + // cancel_goal event topic and status topic cannot get type + if (action_info->type.empty()) { + action_info->type = action_topic_type_to_action_type(t.type); + } + continue; + } + 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); @@ -101,9 +138,7 @@ std::vector> Info::read_service_info( } } - 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 = rosidl_typesupport_cpp:: @@ -112,10 +147,19 @@ std::vector> Info::read_service_info( 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()) { - continue; + auto action_topic_type = get_action_topic_type_from_topic_name(bag_msg->topic_name); + if (action_topic_type == TopicsInAction::Unknown) { + // 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()) { + continue; // Skip the regular topics + } + } + + if (action_topic_type == TopicsInAction::Feedback || + action_topic_type == TopicsInAction::Status) + { + continue; // Skip the feedback and status topic for action } auto ret = rmw_deserialize( @@ -127,20 +171,112 @@ std::vector> Info::read_service_info( "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_topic_type != TopicsInAction::Unknown) { + auto action_name = action_topic_name_to_action_name(bag_msg->topic_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_topic_type == TopicsInAction::SendGoalEvent) { + action_process_info[action_name]-> + send_goal_service.request[msg.client_gid].emplace(msg.sequence_number); + } else if (action_topic_type == TopicsInAction::GetResultEvent) { + action_process_info[action_name]-> + get_result_service.request[msg.client_gid].emplace(msg.sequence_number); + } else { + // TopicsInAction::CancelGoalEvent + action_process_info[action_name]-> + 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_topic_type == TopicsInAction::SendGoalEvent) { + action_process_info[action_name]-> + send_goal_service.response[msg.client_gid].emplace(msg.sequence_number); + } else if (action_topic_type == TopicsInAction::GetResultEvent) { + action_process_info[action_name]-> + get_result_service.response[msg.client_gid].emplace(msg.sequence_number); + } else { + // TopicsInAction::CancelGoalEvent + action_process_info[action_name]-> + cancel_goal_service.response[msg.client_gid].emplace(msg.sequence_number); + } + break; + } + } + } else { + // Handle service event topic + 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; + } + } + } + + // Process action_process_info to get the number of request and response + for (auto & [action_name, action_info] : action_process_info) { + size_t count = 0; + // Get the number of request from all clients for send_goal + for (auto &[client_id, request_list] : action_info->send_goal_service.request) { + count += request_list.size(); + } + all_action_info[action_name]->send_goal_service_msg_count.first = count; + + // Get the number of request from all clients for cancel_goal + count = 0; + for (auto &[client_id, request_list] : action_info->cancel_goal_service.request) { + count += request_list.size(); + } + all_action_info[action_name]->cancel_goal_service_msg_count.first = count; + + // Get the number of request from all clients for get_result + count = 0; + for (auto &[client_id, request_list] : action_info->get_result_service.request) { + count += request_list.size(); + } + all_action_info[action_name]->get_result_service_msg_count.first = count; + + // Get the number of response from all clients for send_goal + count = 0; + for (auto &[client_id, response_list] : action_info->send_goal_service.response) { + count += response_list.size(); + } + all_action_info[action_name]->send_goal_service_msg_count.second = count; + + // Get the number of response from all clients for cancel_goal + count = 0; + for (auto &[client_id, response_list] : action_info->cancel_goal_service.response) { + count += response_list.size(); } + all_action_info[action_name]->cancel_goal_service_msg_count.second = count; + + // Get the number of response from all clients for get_result + count = 0; + for (auto &[client_id, response_list] : action_info->get_result_service.response) { + count += response_list.size(); + } + all_action_info[action_name]->get_result_service_msg_count.second = 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 for (auto & [topic_name, service_info] : service_process_info) { size_t count = 0; // Get the number of request from all clients @@ -157,12 +293,13 @@ std::vector> Info::read_service_info( all_service_info[topic_name]->response_count = count; } + // 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::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..ae152ab0ad 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -31,6 +31,11 @@ bool is_service_event_topic(const std::string & topic_name, const std::string & if (topic_name.length() <= kServiceEventTopicPostfixLen) { return false; } else { + // Not services for action + if (topic_name.find("/_action/") != std::string::npos) { + return false; + } + // The end of the topic name should be "/_service_event" if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index ea6d2fa579..5c444c07b3 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -29,6 +29,7 @@ #include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/logging.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/default_storage_id.hpp" @@ -224,18 +225,28 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic 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; - } + if (is_topic_related_to_action(topic_with_type.name, topic_with_type.type)) { + // The following two action topic types cannot retrieve the action type. + // - xxx/_action/cancel_goal/_service_event (action_msgs/srv/CancelGoal_Event) + // - xxx/_action/status (action_msgs/msg/GoalStatusArray) + topic_type = action_topic_type_to_action_type(topic_with_type.type); - try { - definition = message_definitions_.get_full_text(topic_type); - } catch (DefinitionNotFoundError &) { definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_type); + } else { + 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); + } catch (DefinitionNotFoundError &) { + definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_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..a3d6cb03b0 --- /dev/null +++ b/rosbag2_cpp/test/rosbag2_cpp/test_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" +#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 & test_data : all_test_data) { + EXPECT_TRUE( + rosbag2_cpp::is_topic_related_to_action( + std::get<0>(test_data.first), std::get<1>(test_data.first)) == test_data.second); + } +} + +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 & test_data : all_test_data) { + EXPECT_TRUE( + rosbag2_cpp::action_topic_name_to_action_name(test_data.first) == test_data.second); + } +} + +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 & test_data : all_test_data) { + EXPECT_EQ( + rosbag2_cpp::action_topic_type_to_action_type(test_data.first), + test_data.second + ); + } +} + +TEST_F(ActionUtilsTest, check_get_action_topic_type_from_topic_name) +{ + std::vector> all_test_data = + { + {"/abc/_action/feedback", rosbag2_cpp::TopicsInAction::Feedback}, + {"/abc/_action/get_result/_service_event", rosbag2_cpp::TopicsInAction::GetResultEvent}, + {"/abc/_action/send_goal/_service_event", rosbag2_cpp::TopicsInAction::SendGoalEvent}, + {"/abc/_action/cancel_goal/_service_event", rosbag2_cpp::TopicsInAction::CancelGoalEvent}, + {"/abc/_action/status", rosbag2_cpp::TopicsInAction::Status}, + {"/_action/status", rosbag2_cpp::TopicsInAction::Unknown}, + {"/abc/action/status", rosbag2_cpp::TopicsInAction::Unknown} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_EQ( + rosbag2_cpp::get_action_topic_type_from_topic_name(test_data.first), + test_data.second + ); + } +} + +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_topic_name(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..bd146d935c 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -92,6 +92,7 @@ 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/info_sorting_method.cpp ) diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 6ae633c47e..7ab08cd310 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_related_to_action( + topic_info.topic_metadata.name, + topic_info.topic_metadata.type)) { std::cout << topic_info.topic_metadata.name << std::endl; } @@ -77,15 +82,62 @@ 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( + std::vector> output_service_info; + std::vector> output_action_info; + info_->read_service_action_info( + output_service_info, + output_action_info, uri + "/" + file_info.path, metadata_info.storage_identifier); - if (!services_info.empty()) { + if (!output_service_info.empty()) { all_services_info.insert( all_services_info.end(), - services_info.begin(), - services_info.end()); + output_service_info.begin(), + output_service_info.end()); + } + if (!output_action_info.empty()) { + all_actions_info.insert( + all_actions_info.end(), + output_action_info.begin(), + output_action_info.end()); + } + } + + // Fill in the number of feedback and status messages for the action info from + // metadata_info.topics_with_message_count + if (!all_actions_info.empty()) { + for (auto & [topic_metadata, message_count] : metadata_info.topics_with_message_count) { + if (rosbag2_cpp::is_topic_related_to_action(topic_metadata.name, topic_metadata.type)) { + auto action_interface_type = + rosbag2_cpp::get_action_topic_type_from_topic_name(topic_metadata.name); + switch (action_interface_type) { + case rosbag2_cpp::TopicsInAction::Feedback: + case rosbag2_cpp::TopicsInAction::Status: + { + auto action_info_iter = std::find_if(all_actions_info.begin(), + all_actions_info.end(), + [topic_name = topic_metadata.name](const auto & action_info){ + return action_info->name == + rosbag2_cpp::action_topic_name_to_action_name(topic_name); + }); + if (action_info_iter == all_actions_info.end()) { + break; + } + + if (action_interface_type == rosbag2_cpp::TopicsInAction::Feedback) { + (*action_info_iter)->feedback_topic_msg_count = message_count; + } else { + (*action_info_iter)->status_topic_msg_count = message_count; + } + break; + } + default: + break; + } + } } } @@ -100,10 +152,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, 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..5dc6c34cb4 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_action_info.cpp @@ -0,0 +1,71 @@ +// 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 "format_action_info.hpp" +#include "rosbag2_cpp/action_utils.hpp" + +namespace rosbag2_py +{ + +std::string +format_action_info( + std::vector> & action_info_list, + const InfoSortingMethod sort_method) +{ + std::stringstream info_stream; + const std::string action_info_string = "Action information: "; + info_stream << "Actions: " << action_info_list.size() << std::endl; + info_stream << action_info_string; + + if (action_info_list.empty()) { + return info_stream.str(); + } + + auto print_action_info = + [&info_stream](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" << " | "; + 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); + + print_action_info(action_info_list[sorted_idx[0]]); + auto number_of_services = action_info_list.size(); + for (size_t j = 1; j < number_of_services; ++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..5639937390 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_action_info.hpp @@ -0,0 +1,34 @@ +// 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 "info_sorting_method.hpp" +#include "rosbag2_cpp/info.hpp" + +namespace rosbag2_py +{ + +std::string format_action_info( + std::vector> & action_info, + 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..e807c2af5d 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" @@ -147,9 +149,12 @@ void format_topics_with_type( size_t i = 0; // Find first topic which isn't service event topic 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_related_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_related_to_action( topics[sorted_idx[j]].topic_metadata.name, topics[sorted_idx[j]].topic_metadata.type)) { continue; @@ -171,15 +178,66 @@ 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) { + if (rosbag2_cpp::is_topic_related_to_action( + topic.topic_metadata.name, topic.topic_metadata.type)) + { + auto action_name = rosbag2_cpp::action_topic_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::action_topic_type_to_action_type(topic.topic_metadata.type); + } + + switch (rosbag2_cpp::get_action_topic_type_from_topic_name(topic.topic_metadata.name)) { + case rosbag2_cpp::TopicsInAction::SendGoalEvent: + action_info_map[action_name]->send_goal_event_message_count = topic.message_count; + break; + case rosbag2_cpp::TopicsInAction::CancelGoalEvent: + action_info_map[action_name]->cancel_goal_event_message_count = topic.message_count; + break; + case rosbag2_cpp::TopicsInAction::GetResultEvent: + action_info_map[action_name]->get_result_event_message_count = topic.message_count; + break; + case rosbag2_cpp::TopicsInAction::Feedback: + action_info_map[action_name]->feedback_message_count = topic.message_count; + break; + case rosbag2_cpp::TopicsInAction::Status: + action_info_map[action_name]->status_message_count = topic.message_count; + break; + default: // Never go here + break; + } + 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 +254,12 @@ 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); + } + + return; } void format_service_with_type( @@ -241,6 +304,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 +363,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 +387,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 +398,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( @@ -299,6 +409,14 @@ std::string format_bag_meta_data( indentation_spaces + 2, sort_method); } + 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); + } } return info_stream.str(); diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index 814076b83b..e340803dff 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -52,7 +52,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 +76,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 +83,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/info_sorting_method.cpp b/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp index 6a6e9aace3..02a5adfe2f 100644 --- a/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp +++ b/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp @@ -126,4 +126,87 @@ 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; + } + 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; + } + 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..2217b4f8f4 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); + +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..e1055defae --- /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, + 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(); + + 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() + { + 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]( + 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]() {return this->execute(goal_handle);}; + 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(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]( + const ClientGoalHandleFibonacci::SharedPtr & goal_handle) + { + (void)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) + { + rclcpp::Rate loop_rate(1); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto & sequence = feedback->sequence; + sequence.push_back(0); + sequence.push_back(1); + auto result = std::make_shared(); + + for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + // Check if there is a cancel request + if (goal_handle->is_canceling()) { + result->sequence = sequence; + goal_handle->canceled(result); + return; + } + // Update sequence + sequence.push_back(sequence[i] + sequence[i - 1]); + // Publish feedback + goal_handle->publish_feedback(feedback); + + // loop_rate.sleep(); + } + + loop_rate.sleep(); + + // goal is done + result->sequence = sequence; + 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 e68cdbe4b722087550459da5436a44c22b54047e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 13271 zcmeHOO^h5z74F^H#hZy8=MNHt5-Mvc2Bh`$?{v=yi$FwTN5V#4D;zLtYPzd;+Vpmh zyL)%F=I@5^1#yYQfdc{r9N-27aWLY>4T(sRKqL|pNc>4~;fUb9`swZY*_~a+>sV&` z(AD+o)$5v9uU^0U>doy>y!g>i>Zh6uXEbpwL6K*Lc?!xT?rjEiK#cRodHv+2MZs}G zaP;Vg*#*)QP%T&<(c^Ri0ei?7Fk#@g`(}4MGkOloN>6oTvxu$NlJ;!!k z$J5>BCMybjV~}m7C5kJvK^zqRPF(h3l=Vh<^`}4646@BZ4s;ldA7%L_gRU|4<_7iFHI z7T@*R$Av|``tuvVU3ulTr!N2ITW?|MNxEp&G*xeWM4I5D_AIoOtSJ4>Vxwpk`F87L zw1|Uiy8~AEAKwNelV)lH!%67Y%d&*v&EKZU7Hjr)OGfY}>p`F91aGn)B^fO(lgP48 zZQFv{wk0)AOON19*0(r;-}g9-HnJG)8V`_=siMI%ypi8)TtGrD%iNO%BLGVEeryJk zC?pps8I}%AJrwz_wezhP>Ye`$wo`J^}Hzm1ljh0%RK0`xl07DSw-+(?( z!ebUiEC)w-6Kr{qXMDjv3vFwp50;(fyIo?zVhGs)moKgha27G`#YtR3n}KcM^M>k- zhDn@q9|xlSwCKMKq^p-MuIq;H^g12S zqqe6zRBuyenqd?fW=FTE>)JsOQrFN~$1&`V+3tB}JBk9&vzbLZwBwm<`70m`FZ_CG zn(`;A)Z}#k@L!kMU;p{kqYJ11_Qf}Dxx1Q%bNdk96iZbi?I@Rg1@E%_W!X~<=fcg; zardT5dPUF2JhV|I-*|`|bn|Bg!=c=sR`4ev+Je-;Ad}R6mdi%uhFpN-TT`loTKN(Sr zSpG7wP^?qe*C<1MjgBn;h5v}*sIsn>pFQ)_1BY{W)FP(m?^jm1T2!fiD_YCjH%7-H ztmRnLB8JobrWlSY>uS+|aqw_vQcGi3{;CD--5_W$iiAk9wa(+Eq82<}0q#k`)C8Iz zvWRXar9oCNvLwmKPL?O(bE~kE84Jb~uPJ%Y9fH>)=eXQe!F%o?xwhoJE(IUR?8re} zmy)XyuV1@*>GI3|i&rkMU%N2zM}7E$X+m7Rm4*-#iQhRCf$rO`6tIu7ruN%MaZ~&4 zqr|EG_EYH8j^oN*dR@AUMKJo4V1pw5O%ub<|Mzf{G3GdqWwoug*|BKg z1|2(So3>>#7WN$1by?4|JD%-^CNFR-bp)1GB6{{tNmT*8qw|M+e0SXGIh zOnna!MydM(>{Zd^(j2166*xqj!gtT!RoU&rxZRp_yA=r3KKr%15kB5EyA>9r;1*Rr zuy%XmJ-D3UTF@SayZXH^JjRJy)x5}?)iVC-BeMA=QNo_>1$a`y_5#KTaH(kb>$0xG ztJXuJUBx$dWc`E^x5UM%J?a^;SJc!d@dvW5g4m~5gg6yNeqYv4eiz*dTry-qSoSp_ z?E}l+f~J{ZSyyj6R>!uFfX*82PRFtx54^3b>#l7(_9!}QOo3wOB@*O8k8hO&#ovN-bP?soBh((SU0r1nNP7p<2!ueNo?*C* znTBpSo}-6h5IAi&jBL9T*+$1QJ8eDk_)EqRX$0vHw55Zfz}mRGWN+gz34WHW_uQKJ zbXF1G(Pr;~wx$^&#!1#>1MvvfGzz%Qt%*!@Y04Gd&%Ws$v?j4l+{buo7>4Ay=N@SO zJ)8VJn>Bjp^EvW8!_%b0cpz>wxcA3P1VwwFuer_Kw=vN1*vvfBL8OgL+L+)189oLb zN|or6Y6FCvqswth#=~j3spA{AISyTACMYa;94eK_`5cFa!+CMIa2Ijtc|Hwh0*U0A zc|Ohmu7}1SxI@FNhUekwaBxfQi0ymO=`6X{XC&H65`u|uB#TIi1jvVj2U?OHT0kw0 zi#`hpWYCcyfn>o@oq>rO&&O--BP(S)Bl3cr(GtX!8G)1)$Ti$z0M-Oq9)fF9lCuFz z;cN>>(wrmU?`)#btnpAafAf>KD(iL(ZRFd8k)1iv4u|a>oai<4{He^nc}CNNk-1l; zsLfL`ACL*XBafq(AsWC#s0oKN^P}QpKPq-+JUn^$RT zucjet8QFxIOsp~VXv)Z`x|&n(#iuSKYbDHdvl4MzIWwzv7}11oq;rXQ!pJr&RJL2s zXVn}faw|$&O`DdXnQ7f>Ry@d~W^;*jM>Lm6871GNRwAt#Q6pn&ohaYu^ZP?SkH0A# z41_|#uqWhB8J0ygYGyv2qvCRACZSo;?nJH=P!@hf)khrV*348Ui}wH~8aJ~k6}e{2 zD0-bTb%VXk8c9IiWklnNq!G=ixlW4!tXIr@E|bqiJq-*w`YJw9n&y(v3LXcN^s=BK zHjF4`C~U=&0tj__Rg5xTYIzVP6(wRdD<`-?^r)V&a#^!k$(qR|8h-4OYdOiUpR-rs;UkyO9~Xqa@)qHpwXS*KwcO497;09-tgrNjZI*{$R{4P|Lh zuG35_?Z~97S)H-1q_eo_p1Ruqe7%3@$}oMa(3Id}>srS>Rk1Ky+-!1d2lvG)O$3MBGTDU67A4 zXu`7chSlKCqpGm7U+dgM7R>Q_sO*I(h4N9m+f*tj8c=Z&00Bk$46`g$T{fo}t22fM z(xIe+OtPYdi{~$y<74$4re@P5Dgujw#Hd=$u~y5NInu^zGTEbODlkE6f?FZ9O=!Xx zKIGE{b;gWNwJTv}Wrc^ev@M?FXBD<)v&hz0b-7M_Nke_2qIBBKDbTcLvdfSKwYIe_ zX`2(Un~2meafup(Mpaazl#$j6Ay$ahna}BFcUoqj-?FS_(VQSm8YA`B^sFuo5x%12{u*jgh zVmiqbdZ`Y~4&jn<4Tc5h-> ziH>vwsdv_!kcpz9mv7XPrnWK)sb%I0>r-T#2%84IS_<3(Lf0u>Fqd=%ERt0qex#Am z7l>5_TYknGDTK*)dRwO8->o9x?t*|AX{Vb zQ`588;p=q#G?^aOqpM&IK$$WF#*1(D3YrWyTu(C;!pby64@QMun1U*8z%B~4j@_ypdUq3sYzCVsuKs0?F3WE&-MC#P}SfHVyVY8z~qQ-G` zxmEy;>bD3VJRGIf$8nS%^j%Vf(u7<4M~NucN-U>j^08#XBBL+YnS~MAOhzypH&HE{ zn{qp$4w!n7X&a6xha;L&6KPkLP=1~u!NXC${@0%}R6E;vNUw=mk36s0-5;~=f)d*E zF-yxPV%*weR8(dW&q0&T!uQRM?pCP7k}yPB{jR3bK=E~apogP)&(1r=XuMQ?>y9jx zD1P?UQT?L0CrT)fB5Px-u^VF~BWh)!XNMHs-!fz+y6?{2g`rvh;?70rKJQ!6FS@;A z4?RP|h-Y(CuuvY8#GJ1bp`vZ)JC-P?oaSYrq});GShf8z=WLWxo^lyAi*5n~jwq+$ ztj&&MpG^8*5vq5dwf-bRb@|61vQVOWygRsOf2jKVM3walDzh5BjZyxi5$19(Z-HAk zH)Z=odo&kHA)7yasP<$+JG+#J675sYH2Ot*L1k!Tn4=lVXosmL8>1LBcO_`LiZta= zHpPcU958!TJ8Oz%@3pZ|HpPxJFYLD|f|WHz9E0d-eKfma9R0e&dCJBMIe5@%?+#xZocsPqNaH8@%Q^}iq^`S0s~yV4{;bm zt|(TCpWgi#Yl2_R=Aje_Ju`W)jaG@iVXG3q(fR>H`=Q(3FX8~}3va(@4@f2UqCGrS zVsY=Va+|5AQ17tj+^twBIpFo{+WKvZ0qKR_rWl@jp}2Rr;HE~_1U1|7P&UPMeO13r zF`(X|w<(5W?{HC18$4BD4=YBeDt51`7-qw2LQ_ z8lF4*sTipJmpMEnYQGI?-;la*R)5s4?Vs9gpzNLHN)H)ze+2;P;X{E-U%9+m1Df?; z`vD8-^*cZ__J(&CLUSDc<<#U#BkOhTjNeQ791BLN&C0C(vFK+ihnd+V=30Oho&)d? zvk?8wkLgCrv>NGel$IB)DCWJ`)EJ+NYUI_i*)W%D0?IQpv?yU_XYYZs5g*gYVRyOt zl6wwo?N$e0Rc&W00yRiQb8&cGgS29rn^ri?Yy!(tY@J7}kYQyLE0}4lCt)!I>jGjj z94lbTLM*~rG`EXsE4C^^1;my4=IS2%E6r%7uq3MUi5LwNPdP*BG4oh}?8e%jkz#9R ztj3g~(!3k3k`>glzuAq^5EhK$luhY2^GPfyCRSpJ!ldP2EQaI_tgay^UkCw|n6ahx zHnxaitjZ@aUrsY%m;e`Ouoh`>%M}t@+ICkxCE9H(wA@4@6dR^;mK3w!w;&18z1-|~ zGrnz#+3zlsTn6~0IHu_~!L)a;*F(YdckxvDFrNn3<4sLrEfnws!oFBc4}{|}e?$*8 zsUeRZ*8?#<7|~;1UpVaXc|!qz(Bt<7BSr-2nqpqn8?I(kMj#xG{avoBZ@Pksv+t!J z+hR@g@Wm2~IiF%}P`y{Ug|xw&OB-28+TdO4;x1~eP#gFLO&gS8{?Yhm2Ktj925l4^ znERka_HeV7a*bhg|79FBmdfd>R+_6QP$}!j+bX4ZtEU$Xuuo zDXk@+*|ICN5A<6KE9?0TB-N-xF&0+0;T@zElt+s!C=G)L#Y|&8n)xr_r$I`I)+TAX znH&ysSYk!OW%SBkr!83WErr31>sd9C}Ds@x@}f zCJWV0KrA-8dr_dpeyq4-<=g=D@Dl*6Y^lXw6mM|b&3?pDaAxR+OOcp0w5T}ytD`8b zla~E-asaY5rJ#VF2}O!z11?rKl9;mGV6h@uQL&qNDR<7uTCQU)cQdV*muLf%)dREhLh zqUBnP`EVfn3cC>wrM3A6m{u!*f?n`p8Pc=&1wzac^SU;db39q8K zbzDOrEm{mtJvn91C~sltGB3=x zsWzgc-O5b4lZy}39EzF^3$Me>Vexej@+$&0;cStHPYib$rP>(Gyng8+2H;`V^)`P& zDw!fLNDeLVW?>Se4Vi-hx-Fzt=np3&0lyHQZR9${VIh|g>J`Xs zNU4w1!ArIdZnAaoldXfDSO-tpy09mV213@pK8G6I@JDG10900XZ)GitZem~r(qlM~ zAir59o-|?PqRNbkucb2FBp!Ab#zkejtFvdh-pa0%tw7zK$IMNy(n`zcAZ8sidV zOgykDB@Dy(JQTS7Hki44;*af0P2ErXV06wN)eND zt#-8RkCIltd-4JX?*ARk!It3O`R(h{tIXb0_K(`|`i6#kcZaL%nm9m{V#TCe!BU?Dg1jgyyeLe~5(=%_HYF zR5(%QAF7Em-^9lm=-1aiYNOzoD1(Pn$rRyP@yWj+%%A<2i-m;w`%vwxpLv8W2lXx( z_=e4bL6w~Np6Z092hEjUVhtg4D@Su?6FaBU>y&34c-59Y^aA5B2y$<;H6^(@Xr3YNfLW#L?TBS5^8Y-Ia{M?lc^d&#M z+(yBHIjq4FnlEp7MJLQ>9{3Ck3G)v@^QX`FLxnUS0u9G$-aYeg4D@j4Uu+Z{m^(Gx z`v>i_k}w~;D#${KdHcdj2ZH{L7vvSST^SX}qVx{yaQXG{5hp z7Z~X4ZhzKB!GU?$LG!H=Q(hp<+gDYwkTCxUG(YgqmGppYPm?fgbz)BpU?> z=1vWF^a}G8!hEms2^LDsciOhnFiu_r{er3J%Pj8t#(`lXfA@U--D1g%b0do^a}R z+BMgY`tqJ{dEp5*3gpC7?5g7_?a!avb6z`P9_b3RkTCxQH1}Us zDb0sK!*QBFedT@(^sZ(5+9)_^?$mI*-gf1;2y@@pe#}CNdGi_d4&A;1nlqopA)tBF znyVS;hadJc8wDrLy`?lCd-Y$wPniGX;3ru~nEw+rU-9|_70US#XgE&usW<(Vf!>w= zlZ}Fd=1vXwrw8}BkTBo=g$N5J=F`_Wb^8Wr&W5Q&K=V0Y?#n=**Sxolf|KUHQkq8= z<{u%;Kbaa;kr+K;R(Wg)_0y} zp~QUtzbckoynS>?q^`+qgFS`c|G#f^ zIyMe8T5D=pNHBZ`)}8pUQ(bLmELD4Flb*?y^yEVjggf`6C#Mp7Aa+prrdbIdO=?&a7-#V=pU4 zZEsp}dI~MsZgNk__O?TtZCCg{VG-^;st9B5`uckD8z+9atKKG|!7~}i(dGy4FW<+8 z9kGsZ7xK_TS2+jz7JSwEfVu0z>V_@{YtD^!AR){eE&~(;)uE|9KeTD z5Xlvt4)l>u5PWM+>CPO|QPpkuLl1AgHMvU|I**_E0~;+`A}7?wtvj^e)}vCWaoqF+ zPsw!VGcR4&LWKF@{3}>UY5#>{AFkXi8hnG0_bnawU(Z0ld-+-$1*e8}7=(KJwEYS5 z+2>4Pp~SrAvWoi_Hmf%Teaj#JFr9(E#W5}dClnSQ#1NVGh#krC^VyO zOH3(eOO)<{9kC_Yk#$3SG~Fh%C1`@f<$C6Y`xy&&8~56*Og$W_unU$fRLGWCyzhzY zSmHix3F%-)UN-WvJ6&GQ6Zb#DivL-|!*(^4jjE~rYg@n7P?7y~x@$gr@eUHk=7XPQ zA*KBS#h&n9#RIy*AJ%XMJax~P80hfBoDR7D~*|-^sb2 zw1KXNO|cJw0={CK0~zSo?Y+Q8!HIdO6!SY5pYRc3e(UWy782%Pg66MJs<_h~d~<`- z{DIR>WS~F1`*Ai3PRxU@G5Zq#;~D3oS(Y+GS#glp{6x%5tD2UGwqxV>0vv3`N3|?7 z{#1QILWE}={%b(U2aY^K+y;?x^MYOO#5DwTQNv^BkoU`4(NRHgq(^*JzL|j6$ z^!;$h&ZPZw`;HPm%g&KxD1Y4^vto1axZ|EZ58USu&91S;UwBH+Tl8P5u;RP#+tIFu zsu+Eexii^I)JJeWWTQD6NV;nAuFcR4Y%J8VK@B^L#Bh@DQ)FkPDp+q_iF5-RQHlz3 zIB%A2L$Df4U)V=J#BE6`0;>E7vkvuFABsQp@K*hIWOs(RA90W|O4?DEo8?5_Oy@ic|brXit?$>#C9$LR2B&PI%jgytAAm(mfoc zyGiplg#$5(PV@?k+>Exd=Tt_FA&x>HGmC*kw8;CV(8(R-=pbj z*l$!f+l)d#9(ZLoz6y$+eJ|Fv!K7NmB!5?~RT=2geP%pZ`$hEPf0ZY2kFNJ8< zpsYreRbH(6JV9(k(uRYisgR}!&hd`mjBlUejrg=k+!G4=b%<0jqQ;uy9@*FoRda(PK#*;_t@KIpxz8R$n{Fx5uEi8*F(_a+X&!`bGe zmtGDM=I-w2SV)+cvW?uFD%pmaM#T#Gw6DL$vaj~MWEVpX_+I4VElweqNX3P9$2Cz4 zJ#s{zg_1X>o>sAA6Z7dB0;yPf#0d=a+V8BfQE>7`q*N+)Uh&J7g!#&o*Rqf>{|ZEX z{D&1gHVyui8kdUQFS~_-e%0PL+bB3O$CO$zJOAnM^eV#qnr&CIP-6bb_s-n%*m*9O zcOmfJ(~}IP<)b=m)Jg@uTI47ccnyd>hmTERp=_na!Ad2=H~5Zk#04(b z-!I>Wf&Rq$X*LQD{_r^*-`>a`a5Z6Wt)I(6!rb-xQytI6D()`^zo+DqapaDR80f!$ zWr2-?19LBW%3{gbeV3Z+3GRk2WFzT4U@&+@LG&v-y~g}YA}%TPM~(a|<)rII1% z^c<3bl1ii_zU_^ynA;Dz)-H~CMEj^x=Sbh+`_^&kX#MOa272S18*CIDJmPcs)*XED zo39X$%sb&F77~w)0_MKYD(*K1zsKa#vHgv2G0@LH>UA3h2j*V%n8ng@-&^;+Ntm~d zzKewt^QrY$Yt-ge-_Ht`t++x#~Z%E_YmR|lKEfdNw>>lg4RjKue0`yNl z_LBYhGE4Rak~^|w-+P1b5W8b;vL$;u-VSq(a~3mB+O}zt%~8Z5rSeA{c3r(zL)=!f zWMBW?c9!+kD-X7dpyc-+bWEQzI2@PktK>ragfa{5y}~fE&_1kh(%xhX?WB%ft~W@pA#Ic54JOeiUhHtSVPX2%wMMV%l-hI`oJ{2yFWrm|?M#)I|!&h&+ zkj%!L7u?K3iCX@WhaG1-^uI_v1k+V~ytvodw=>YE9CxdYf^*d5n65f|s`4Yk{0854 zER>jcA6W53-Qate^YP;PZ|ufE_b%MUM!|_WHnbJ9bJsgl?jy`!uKDa<3JLRSuyf7! zU0WVI%awmF?1#KDiJ^AhtN*Z3D;4&#EC0nC-vOT|_FS$%zIFjaY2upi+NhOE z#Q>Mb$36Yd!~|OD4eK=)%2xWI^7+NVpJn5c(Kh2f271lxw`>%g{NdP(_3uSXrxE6V ze*6FyO3d4?t9VX%@Eu3EWbEC2Fatfm%VHY^C*}^vktgroD@>Rl+`1cUf&8>6Ph6+NhOEh+Ihl)`U5`t)Wqp< z<%4f)a4C7@fQK3A)Bf;V8wDq?IQD(VjXl>9=Et<}!a|ApEyq+k{_qc#`{iYK)iKcT zx?`4&f&+6uoJfktAMcKHy-k=m?DhFu6cXlcNXdo2*aEGrwjwDx`Ry+lO6R2iX`@yu zC46JxZlyYp0}~?LPW*YOM)pE@h!?wcp)eaep%Sb_bV~<||bOdgk=QZ4?~* z;&W{8xc%(=M;!yqA4}iKLW%j;Z>v~R2H(?z@WaVjC#_?kckX(hje-MnKRi98yA@}W zul@0wnS^=W_E)fwFek-Y%2IL(jk#NOu^L@2zRGapmshf^9f6i zI5Brj>Ur`jc3%n1yPr9Pg@pMO(0tXZisgLpH5{k;lP{_a^rQ*oLf^o{!&=qJ_v)<(gJxnr8j^S4h|5axj|KVTtYz7=Tx z@B1pI`QU3fPV=c}{)>TLb>t_#D4@41y4u9=&~PW+w4#GBzo&XR3nk_cKVC7-2Y=Fq z)BL0LDg*tOFCN{Cf&=rAt45B7Cb}~CEL~)(zJmxJxr+Xevv^#tAJ%-I$K|TYZs+p2 zoG0Gr0v?yMV(JfgT#o<0b19G4j1{xS;4S>Re$IMz1sXmfMpL&oe8l2Q-8}i~<9NK5 zkKJ6?|N2w*eH%VY?Yd{@onn;#GZBAx?k+6;CimfWY1{dZrSHF-%*(s;to0{R{Kqt; z!t>LIYEP#4g}f16vsbmV_~o3PTu)9qp2dIoHJ<+G+uXq7S54#bPw#$=#cw7DXguF& z{eZ=3tGMi&3gwmg1OPaBKhxgC!`am(7>RKRop_B=jq`a>*! z*A6`1@Z8x?#qj+XbQU+BOJBLXnr@Q3gw6oNQ@vNXh2mE$Eb5Y%BmdpUvNdmX^Z0jpXHnaJ zJT7nYS=7qo@_w87Vr@{~`m*l={$1XM(!4*9%Ns%h3wc~#IPtMqJ(1Tiyni77E{|E? zbt#X_L&Oh0n8)SO+6RhRGkM^2Ydilgk73?t8IQ}ui6e*bxI7dnNxa50}_J$>Z|K zg=r}smj@9{O7pnfa_={JTy8|KC*6ccZqJ^zipS+9yYxic=}aD%{#n}y^((!R!e{gE(nrVlT^^U7D|O%Fap|McCe*d`Tv+r! z{JUJwpMMUI%T?=)Q0HlR4ez`QcwDY*efL5hm+L*JU&P~bHRBG^ z9mqNUUtG+;%bD;0y@bc*obD@^^0=JoeBp;YE@#g!zKqA^w3GQ`9+#6biJ$PeoTAV! z=W#i3KkBDEE{C&+{fx)uAn)KScw7#B4*WTf%K<^_l{_xZx^ zo0;}MW;Y&}7p|W#w7a~pSqt;;@-pAUg!Yyfy&fpEwY&s$AEBM)1(E)Jd473$;HDUyZUFC&TV}v%9mlgeUA& zJbrEMJQjaU#PzAIEPjfYf4_V30v5;qarVedry4}(Su;~`_f(gHU-Dx0gs?^ArQ1hE zcy@Uq^TEQ_keBtg3fn?n z3y7PV?ht*wyj^3u=*#8J3*$uJEe`@e^IN=rc}Vo}NAS2j@#@}VcwCC4f+)R6`=&Pi&^xs6^Bi%$l7k!I#j@w%F71ABZFZu%M{BVfS z^>Uf_SfR`1V%C*HSIhZypU}l}u63r+y>cFI2cc`_9L;2*TjltDwBS}bw);frQd@Uc zG2JN-ME$GE(U&{(J4AEX$)~X7@@)L7Gw%$m4eM@hrLAr*{5H{Igti`;$;ZcJc`e8=5BF}1^I;ng%maqSO|Yfh{&^k~Y+sk)j|m*W$z8?tcyPqPwn zTR72K?J%MV-ALyW@r03WR#KXpaqIcsa}LvL+O!PKOzT#&f}^wj!BE&62~ut~ODEMv zbBUDE>{gV5lu;{@){Ll;F}2QS#TyEEBLR;$;`fHUzKA#26m+Nfd6ZGj%%^kBiq9K% z=gdq(v!dOJTqhv5ECP*xM&b1e`)s2^71EPdQMtlTuVg8F;zO<{gAY~?*jpV|BmOkD z=dyrTe3#9mvdfU-W;Ue)4o%OB@8z!il9duToe65bo@Pbdd?vCHOuCWD0c}sgtE`bs zsIf#ckwXq1Ng4&;sm0~zE;9+Bia#5wPP>SD0ib$77Ld{B^ZP?SkH4v)IJ67rZ-JlT|g`iVl^u&4hTmZCbXiup9l$h{xd|>E;FIi<%v;Z z0-ywr1D|#gGn;8Z6vt`hvXO}$qPk&e*+hm>yi>J6qqc?nw|l%%J&uDkwNStl2>W6& zJrIt^{1H9Wq=r0tTo1(bU__63ePJ9F4`%iUJ$_tCW<-z*9gQUL^ynj+ z+1bTxLe0Ff*ETWMD$&ejB2>WPxZ`Dn!$)k7A|qU!X>;nB4IV|kCT-hFtng*@L^1e#-^dvKr}YL;-}zJE_UKi5o2%Q z7NB9z(IcivMWfwK$k-cuWD#Sn62=~YDUwmM7~bLnc5Yu=Mcka#lBFtosZ2kjp`rEu zJ7+scTaHXnG4jg^s4aVf$`kM#O}ho+(&6I25o=$Ce?@<68|MO0>_&+|3*-mh757m>(6d`JBw z#v^f!d*8lG#}pjFbx)uAb}C$tA8a^1p}A^!=$a5FPX=6P7o|IhrbWRkXs<$LhlW!>T_@B<0TSi*n;Ba#wGwAIoXk-97;?RtKvi||-#$JQ9 zN#_wqxPQdPVN*r;GbpP_n}_=~GnL6=s{=L{MCE>w88h64uT!RuO(%GXEgskk(Pc#A z*vJvhsJTvy_FL2|n4ihyb5TzN+h{{y#Rp2$T=JP3&9TjfURJc#hB6fH=qV|ncvla7 zEE&zy-W)BD-7cvpZ6az`PH@wv3mqF^vL<%3m`Q8_>6lW*w*qTGXYir%LwMsku65(Umqa^VosbD_`8X%ps=o|f8)@fLVk~BLyP$y4h zDRF>cb}P9~Ls{CB>on6!J2L5NR;MhTxm?C-Zfw*|&1%T7?+s?QqtV}(OvJKkwx>}P z*U*_uC2K{bo&mt>ZAt5jnl+R}Dw8x)@NqYEC6QLLS{~bUEJd}HZX=m2tf~WhnDVg( zutp=A%GW58HpY@>tkECDJW4p|@iYYtkLLHQ0Z)@(i)oQS$cV+_a41(XQy5blv4<*| z*9|+j4QA~kZ&RRtkvHVyO_H$C5^9%WDQp)E+9Ru&XpJm!4{L|g7R#AR3oEWRcvH3?AWN4t2?X>ZWvZKAI%`BoPZnfp$ zNV*y6duqe7%3@#?7h;oxr%+OAkCMjCx!5VDWU^)#wzUZ^ zN~xI)*u|nR*rKF$5`c*;5QSS+8qk##MVPqVwaR`*EZ?D^bqTZ#d>65^NJB0z{Q$mB z>F!LRI3z)X#PdnSjWpT?`51#HEGutV4emUu3M>1y&OKzo984{u?1da<(K330RYBo)xBCse(jH=ZfYqgA- zqfJw4GTEbODlkE6f?FZ9O=!FrKIGE{b;gWNwJTv}Wrc^ev@M?FXBD<)v&hz0bvd{t zDQT!lRFqDeIc$cr%w(4#3uH+H@nj^ z`}~$=EsN#^DLYdq^Ako=Cn2ULUxtl}XzEmkQ;C5hbe+--*6uLU#FXHmSP!EMRng#% zlD9p@9P|rEN(&;}=`Ukw$Aq7P~hwtVBn;fz&(eO=vT$hTgnU!;IF-D5REcjAea_ zY!k7M7QI>u+yX+^DP7p2t1DoUtOD^PjfBR)OQtn=47p?h1^vFW5TUlSYP!Qvj_}ke zUL&#x=Qflnvbx929>scQ9GFfAWB`8JG+fBsv{kmm&%dCneixZoH>@yTBeO*@@`7ljUo2I)ptmP4Y?_LaKF?BV7OHpaCZ zHJEcjt=QwXpsm)A=WSKQWa*rWeiy3DcI;Ic0O)j-xdvCiBx)s}S(~a*gs{WV3FR}e z#f&->W1%AeVp>54LJp-t!zyMPxU;SvKMhH#TxS-l$ue^2nwS`XZnjD*(2k`rn3|S{ zuG0sg6z4+|6}CDF6k`^UkLy*bzR=PH#9|QKu8;af@DVy01JI*O1+1(Ww)>9;_fqNT ze8f@c_q$%)dREae1X)elQ<_@y2up8k}>c$$-ovdJeK<|fTNYCCE2r)~{>)Kp)KwrdS zRRi)v&_z*$>a^P;MfMACQ_)Sp{>>?SMtKXPCxKxphB!IW-8R|`yQM8{EeEwOYi)}z zUa~CO-ntCb(}iupR9mr%rw)^>VW>G2H9HYkI?No5@;%4|2V1}{NyB{P=Xp?V3}#-x z983Xtm;hy485x#=Z-`F33qX-JiRB;$J*hBb!L%l0G>n_FW(Mbo zJYujJhpI@E&dmVZ_Mpoa2aCww#;%Ezw?SD#of0(wZ_D0_sUzns&Oqf=b|=8PtS3Nn z7)u317{=anO%tQvG%u`<^?G1Y1Cm3wa~4(t+K`AaJohvoI&ewzD6C6C8)!38g*e$7!8E1eSHo!xM8|A1pq25ySK6yMK|*? zG!ZO)EkRRBN<3*|h=VFKCXx#<_>2a%s)T}p1~*HHLU{>=W#+({FmFU&rZdts z1||R9bV`#8@tOqu59gS&q*6XItbiJ&ksQD=& z`br9-EO#H}6_N-x2T_2s5h41?hDnZC0&204Ldeb97TIO%Zn%VUZH$6Mi=rseru~#8 zT8(iDvhpZ$ie@a#Ii{W)jJV5USNtmUBP83J7I4~`Z*2>;w@B+6BaQ-dRkNdi3Uih7 zKD4v7h+kt}e*7B9DpJWl3`a>d11_n=vE43dU-4_4i_Rk(2u#H*TXsQz6u-v1W|c3f z+;I|Qfs0dl==8TuZe6vv9omcu7>xcgw@XZzjpOVYtFagsp#oG9#@zMw_2RdcPr1i5 zigB2mpVs6&sHk8dQG>K%v`RgNnN2`RvFREy(S=bKvmFFfa!gA-1-C13CJ{6a8$VG2 zab>=_y2t)X!>ANyHg(p=fG@x*zClOf=DC6ZHhfs(b>nMFTN(VzO}|E|4{;v1s<$ zUG;!yw^5i}rY52pR?H2J!B3ptbRE7<$Io)dH+k``?7p~y1DpCfsHyOPCZ}^^vC~Sq zspMERS$Hxo19PgHg2v=WZ&8;VrDKy8;FUNyX=A+y ztDYCs9F6%1I<>5kjl_!1B@mRor60;RZz9ST?D*P5BrQ3fs~j7Y-=R2cNW8K8`KzW8 z0r8B%e^U`Q+V-H>(JTo3J`SerOLX)2wkbsSzmvf3k*0GB-lkB08@nyH>Mf!Es$eVb zM6yxfC)(>3jg4@T5jQ{3bAc=cdB z7Nj)3xQ;DVzKEyE2za!B9;y05aKSj&I2X3N<9Fx4iDUvz6C z3AMaTTVCr(w@^-G?kALR6uZ-reoclO1An%1?Jj~vG?U*qgwHTJKu zm@(kbB>UZX`_}~f*SN<-vQj{I7Vs>;mM0*c^&)$ROOm200n`L8LiwD>j7b(#28<9bZ|8teL8{F>xCT+}<>b*A{0 zl`V~;W(gQ&IB7>-zH3hrt{3`Mct+926+6uvqQJ8QJaIvq$>eoF&zKY6=Rc01U8#8N zFICT1^Rn9oh3qdA>cxhc-|;NHdB%MpzC2U?Q-vFa#I^41wni zfg6d$)a=FBfo`f~-_^H?fkNnj9yz`x>8<3%d6j5dK0y%ptV&Ls-a7b zDs*Mz+Fbp`k}2Czo^;#wjmdUs^}6uP?uLWqlKU zb7^zlwQ9NoMl~u|*UD)1TPrK`%|XLa2*iiu+yu@4hq&w9#P4DE;}R*|eDFLyk+ac1#yk(!9?@$$Pq6 zZ7^VdEts%?ylSZMc^*0*-6%a(LW^VfQg*e3G;BCzCNew6 zc+3)7Uw&mHosn}y%Sl=xlg{IuqG^(r!vzQ)K~geOCQ~3p$fPqlDVJ7NMakt0s+P?b zwQQy+3dMX{le|IN;w%lq97-VAaY@Rw38zcp5K6e}GLwgF=BLn;H_gdoPvmQ2`0PPtz zH2XLfFFnGA=Kmw3A9IsG82{tB+2lT)vW+3Y5MT%}1Q-Ggflm(tM_(FAa^mKTcbDta z9VqMmSZW}mqf|4jC_<(gPFsm)S{LP+J>7s9g!5x8vnOjKnL*azm0Hsgqbe)n=JDQJop00M% zl}(oyOUEzs!LF{;PS*=VN$#aW(l))IG`m!cJoD4uEOs+?HC54Z$FWxA(sl^H7lIJZ$0f{2TG>yREdhr3<5KnmY~N{H@)@{T9)_DzjUo~mT>5giawrG^a7VMK2q(E<_G5`|`8U;#)P z$QBkBc!onW_)bYS{J0bw;|kO7emNR^5q=?vyiPOGK}P@bAMY4$ z2F<)e48uZumTjnCm)@ zyis0XUcD|~TU*^IU+(DCS1te;Rfa6Q{~0kLPh zZh-b|cpTi|&UqI3KhJ-SOC3%=n!G#7PrN%ZHvW_G^XKM3$To%mLx3T`5MT%}1Q-Gg z0fs;;0`)CE!6g$%U!8jI)v@3G?X3s&SE0inhNkb9qB#t0+K>e)MT;!R99m>O^!{&S z)vL(_7Xwd+sH-1;@(4Y*uI`UfPwThho~LiT^&RSI9A@SIzm~~AxW}XRS-;dbGG z{^>BseV$f%XY@$7a8mqIe|xXZ{-{6lus`zojp)d0{G+y!lV>;bVZs~P@O^&$*Y`sw zsLziN+DC5n`APh3$LVmyH^6s&?ZTy~0pHgDbE4~KVZi=y1p9{{EkBN8U;XL#I~ouj zIC|dSoClGzANNQ0uz&dA@t>k2-(SaVBm3A#M~j{hH1gq41oVdw?x|7GfBBbNZ6o{f z7LFDk`yDaVi?%zA`3Cr|fB3;Kq6S>}dpqc@u17lxJMZThZO20cK6S{Y{+aq~>d&d) zrhb+BF!l4)&r0RC;PSvbEj+9&`^m1q=zF&YUAl)V{l6o62PHw*XFp>y#>k`DS1_=f)im$~C- 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 0000000000000000000000000000000000000000..712aa9aa3fc0ef9ff1ab3399b3e92e5380102053 GIT binary patch literal 90112 zcmeHw3w#vS)%fJK$!@Y)5D*p60W>@e$6ia1V`;_)pnyREg8~Kx3%{6G}aXW0K@Z9MHC>U5d)V z051f9yzqnq1m(2*NOARcdpHQx?|NJ2C0KI^neh$E2iO9fWLby9!`dD0wC;c|?{bg!)14{F0br`c5Z>;8IGWwA<`@oz0YFH zq^M?yEy%PXwkJ<)9jHM2BDTDU(~PP0DSy<$dJs>-WBNBK}J0Q$ri42yg)HD1(PlA(~huPnz=A-hG!%{c|T-OqSDk^^)u=a&W9Eqm-< zzvz@%!7e#jCnt%#6SRfb>GpeNi4z=lQQ|p=GJP@?^1mtAep!|TUR>2hwB@yM93u$k zQz$M+{jl`JYE$!|As~c9yeI~I4XFobDpwYoLW#q6d_bg8`=72)bs? zAdbCv^K0pHix_T;L;({#)}S0DsG~=W=yJI&EP>4h7s0{-OQ2o$;b{V2gpaqz(A*eD zhZFHgBJQ)-NP@#cjv6S}u}NLmyEx6d*_e1S%_8k;XJ$haAILO1(mGJBv~JW86JXx8 z`S1us9etHWKLr5ho8teM06Pn9AEU)zVf$410|Db>P{5#oK>>pT1_cZX7!)ulU{JuI zfI$I+0tN*P3j7C9pu7^0u$a*QFR*=z2$_{%_^0tPC}2>)pnyREg8~Kx3)pnyREg8~Kx3<{hU6gVU4g>m*z4u+ro67a*;3_mIv_+jmjU*~w%)IizZw!Lb5 z(YDL>lu6z>m$~CthZRNwXU(Qv@W;K zwNAFWtz)c1tu||6#gU5tsd%m8_Z81q{Gj5&in}Uqs90YStMFGWshCx9Nd;SRLB(0+ zUsRNre^~zK^1bE1DgR0N_sj1uzrB28`4#0tdANK*`Lyzj%4^C8mJcs4wR~dvtL3oe z*Opf-&sZL{Y_r^IS!>x~S!G#anP-_|@mR)MhFSVqip(FI-!UIBzhwTod8c{1`P=3j z&Fjrkvt(Xqo@TBySDVi@_bdCzTv+y2*}k&hls#8=Z`qErTg$F3YcFdpn^pF;vI%AO zvJqtiOaEDBDt))~KPISc>-3#_si!q)TFD^lhbpw`uPdieJ2mK?P&M@f6p+TQ-m`6RPLPG}b zI!HaLK?6*8Qje(6z@6_crXJRyr~bN(dME`Qd~3mxJhv}o%^)PpMY`i)!OrykIt z8=5v#-&3LeLw|iK^<51*XZKj@eieFn|F?%w_i4}pVH5Qo6}tb;EjH?24f@m6dDJ!) z+Pay)jk-sJZvN>UYO4x`*_R)u?$#h@>owH3Rj75!`ZuV%G-&ej$EZ71i2rlhGt?a# z^u}NMQ{Pe{$8`@cpl;WojgO9_wy03`gIC;5-KIegY+$L)Dpa*>-2&=X4O%k$9qN`8 zbl@K!e6p0fS%W_6UrybmLLZKN@)PPt4La|Oq0|j3^zIYme?)y#gC2Y9Notb{J-2-M z_0%^s=(4~|)J7HB85y>mx?Y1qo35a)Q=!Mt{Kfgy*EML+Lr16$DzyHoFQ!x1YS5FL z)=`}r^!**XscST7$6^+BM*h-}@k;0_SX9 z*A&-)yMFsjOat!Rcvn;f3b$4DU#$UK?4+dAiIK-~Ygxf;;6@`6SUh}=^j zux;DVC#%4f-}vx%lQiJ#>)x5D0iEmmU8(}Se^}8a8X(>A&IA=$TKeaH<2Asv@a&6K z;DSH=;MR*Y;FU3dt5bpV+75r})qqv+N4ONQ|6O13Q;!BLx8Celfj3`y<4KnW?Edr} zrwZ)Y^86PL4Y+OD5LN}YbFZJtXn=d^XuAr;s4u=zF0``6So6zf{G~k6%RaGi*WL?9@7ihp5-`VG@ zz^-xi(?)8*gg6wcIuU;O2~K`O9x z#ia)Zrhr!(Z~6br257*lT@Rh10gj(N-(LfYLEuzi&NJ1VO#?35eQu=&O!(z$s|NII z{HQ_&X74*ZuUrEj_`cne0`}Yy{%1e42F!bQa+wAct`$mE;FdWH51TY#R^W*e4LIlQ zsl^)5e_*mm1Nybeh2=#iGpsUuwoIAeBl-VKKc&jX!VlwPP{5#oK>>pT1_e$F3S1K? zETfhT>1;_tKHHeUx3KA{j4)f`qA28oY>Gh2!g@@p2d!ARJd$p1g`&5KV35X~J#^Sl z$MFK>`GG&65bhd222dyvYn3IM4@tBbg#3{bWGlktiz$fY1+rLr=qx0!$>>mw%6uI9 zi-&2*@CR8Z6Eb8`6vI(0)}(eFj>sXXV}wIV@4ACtoq!TZkl|9ILy(||q>NO@EhB1> z{1B>aMb&<=5lJa%No5Cg@-Dl_;gDRM2pRum2jgYDyv&K5i*XCG#Csg9Tk_aNPWHle z*~{^Mhad9>I%Uzt%kG<+3d$(&kgZFB)?zS|&a=WTDs4wvMvkPLU`#;z4mt>b1QB1z zf)t9!@E-mL`p0BC7;b4ndXh#!amYy&UPD+hwO6G9CnjjBcLYEw;Xg?Ho6Y@vsU>0x_Tj-wu_lNFGS6%1nnr zFr%14iqIjGVjxNaAOv|bqbY1qj@tr~1d=mb0odEh|S zffx#@YOrM!rar~TfnG?n81j=3riP>PO%5g6;4-Gr(}#$omI9<67f7W>Y|<)5hN0l3 zc9PM8ZsR-X5F{PLc><-p+99`VN{ZU}NCd@-=zM5Xs8i4Mg2@nMI0>E+z=mO6X0S&}r2{~3{Nx-W( zdP@DI1yjc}cIFus#}v$9j5Q95Tl$ib&)`p>P}&Su7lEm(`By=g)7PKi;X4 z2+ND3l<)#1PosI{6sRjih-kA|Ya%W|{#o_$hKbD+XN`B^<3yb_1mvKE%7f@cwUD5~ zB{Gy$>NBFaTR?+?P=W*r9fU9ViW-ZM2n0dtrfaTd=n1*j7QucehogW+1d;?3ofJ;# zrD_mevlJ@9?AtuI!v=J0~N9Ioj)3GtI;$z zTv7WWOA|b%)-KcRk{X(K`wzHxn_!-76OAe=B;9YlkFnO@wcm7G8sv6CLp5WAvJEA_#U?U=`R_XxLP5p6RUdCIR<}a+%sGEjz*SA~Xj@wD|so#QS z+_-V6zhj9ul?4t~WF*E3czZ<;BfCV0%f&D*uagx-pp)eHa)Q(0u**)#YlqD1g2yF_ z0wa0_hbX&6?zWXhNRJ0&Sskspd=!>>;7NUkKig#m48?hH3c${asLg2*!%!SWauUe9 zDqb14iTMQ6g@>%MU?C?WAX;)YbQ1$B6`W5otv@t%pf@o8=x_+^O5%sWpR($e##^Ic zH^t;QimKuyW03{L-bypwGRF`W#RM2)QXed~2Jqn1J!4$$i9-*QOELOV4m$!kh&kYt zYwts&9V} z?Gy*JhoH+;vp#n#$wA=#QM*kw>(cFUpEHI{M%=+Po9qPHy&2&aO@7Fvf@u%`(92Nm zU$U@hCUCkAs7Wn)IAy{8f|(dih!E_8Cspi_r*HU(LV5cQ}Gv~yERM*c5^4rpE$d|xqiM6YQOuM>YIUkQp)imstx(5QJE-r zbk#x4!9YfXNzxLIL&RVV#s`3#;51>GdaSgL+IJ*P#;m+wjr73a!4AnvWq6<&-iLfr zmH_Bdd$}56foNyS#|EJd6G4NZPBa{W3h!}@zyij=d=(&B2pPi?0yzo)iv) zVHQ$4t0t=+h^P!R2Oaw;L~?h7y?kng03B|3h4gRqa@JT#L$Gm_9@6`agJePoI`u5k86gbNp$-5G7@}OE z7SJQXKwAJrHMQ<4ztXw^+X9PbAlHNraL}vK(v`@n8w!WUsW0h5eryP9CI0e}r+erx zokd`CAP4XLQLO3}2Jo>EjJQUZ@E zAUv5OWBQXRIlEuvCCvdcX!-a)?s5N+}${>XlT$ZSu81 zt(00H#F*OySV;>_&^|a8z9x7U%dGf_kqhOlvR0}}t5DTbWvcXwRNd>Kbgh4qt$0-T z0#scKPxbW~wSZLD0#iX)Je9&zSqn?`t@FOsD>+WSTIb>tGc|Q-8rxI61->7R%Uyjb zgSDAO&$`Q6POLpvx3hC`ady^1aU_GaX=UN^KWmSbwKrT?WTt8x2i&m)l(l9kDe5?u zVX%TFT{~BGa*wC_O)Cia71FzwBB*<cxB#Hf9g}1|nQ~A@mqS8!Fk0~006wOwG4|*9_wc2H0f(;o3E{~Hiex)@BY=FuhhWAS@ zR(5)M&MUec634n(2UMu$oD2s6Dm!my{T?{IVt2uAuhY85az zBf6wBxJ8cpAds90#*gluD#B5#mo%L!$LEu0jUdEv4npw2NiUo_^1_KER%X1c==Iw@ zz+(uU{JuIfI$I+ z0tN*P3K$eHC~#U)ptHKanYzG{wlx$FM* zU(*olj`Q}+F|(`DmUVkZkNQ9|g?J*C9>K)}g&4bSB6lX@>Hp(2^Nj5o#&@jSGwAR!q=s#36CS+TwRg>s8!oOw#w($W>C)g|%bHAR;fURiJ(G|A^Dd3k>mWj1w6kf-pK zM##HJ6;RYz%O6_IKY7P7kc^q4)RRE?E$t|zKEeTFO8usU5EG*TNNPmBVubpi9+mzj z1zlWwj82b&ewM+1IJXjxljJ|XAbde77Wct{*;@RMUq8nmco<3j?IR+IiCxqtQ8*p#PXURv%fXD0d-6CH?&f@)r~2A=pT`eL-I5a?f%2 zlgfE(8uE}MRYm@V|NV$P=J~B7e`?O}%PZ25hn%n~@=r^@?iKkyab6euQ?ukK5nKi|EDce}*Q_G?v;KRJ1EMgYyL|uZBqI8? zQiACDgrks)T19ly&g*+cbaam#&HOgfNkp#BgveckCg&c^t*s(=;h_H>-J7_g2|Zs; z<5^Qv1K@UIa8Nl$H_e?mxn9XY3h9>srCL0{3>iT3!YN%z$R&EI+oCm=Bsqo2jxM(`Q$=4C{jq3x;lB!} z6b>zDDY%6C26WKlr-~_|3QQOG1Iq2k3KvK@^n9m5uH7)bC>#xf&@y%^$=ZpWG?Ru< z{Q?^{fVuFS?|fW}lc5a$$NAtYIm%EttLG}B4+N~EM`iSevw8;n2gkfO_g#|P!m(QX z>BDqSF(0_)#B(Rkc&t`=9dXtvsvP=*v^kJjG#nj02KolLs+J(vAIFSEI&jQ3xZY(r zV9zAk8XY+-;DHV60lCZGYp(2y97_#@4thq;iMZtlxp6w@u+B(UlC9Q}3qlPN z6}d_O{Jb0IdPmNiKjbddIftcEvLXiol_Wb>N6rIfRIuR`;2dAHAV$Njn~ zN6xYNL+%2NbNjI7PFCbRh}`))az~&ZkBZ#ilBTk*3WYg}LhxvLfe3NpF?w}T%BuJh$<`BAoP@Ehv{6q3W`*z1bEi`&m1+SD{ow@8mc3QwY0J#N34?} zI&zbs&K5Sj6uA4dMPKHKTpq2Hb9Cge99UN5$O3-0j+_Zfg<-=>0J+Xz=DNn^aREO| zLylB5%ZeOX;|6QU?forOJHv((0J+Qlo~!AV$2IOu9XTw@mK8a&3=Gndy9o-mVZ-r& zTL29gU0;-HD9+`AMC%i*9ehCxgJVr)HXO3;#h(UgFO?__Dpq<;WGOxcx$ zB6)}yZpmI`o965du|Dle>bn*ve^+wl83a+XL(vEY098cyJTkmjMEkTup(A=q>`Fq ziRlk13g6nNKt!suIDDf7Qk@24wV4;R_=G_5VMyd z0i#5?1odlHL#>WfGGua`ga%_h->l1wF;&Ajt7ens5@SrSVmV&KFM9xO7LivBpuM^e zO7*IQ*i^l|o7Z&81kh#@H?u8y9Bs>;|Ic68a; zn?|BKA}&rU)2gD6SZWAdpB{oD7O0I>(ozGgFaEP zxHNX!Ef`DUmG!zT-3^7ORZ9Mn%{3UC$9UxweWGA}Y#eiUE#QQSGFeCNcBq}LBKOYt zkGlDCN0}(Znaj&WnWRjVVw@;g;~NK!ES?VXU3L5Vu{cpYZwJXLLs3VYC=>OGawXIV z$Ih^$=N(s!%|20>9w&+eCkn*dPRc~NRGBCWRiNB7A_D&iGB=D$^hyrW(x-9WfMqQ=k;0Tp6cQN7$3&^RNaq|Dtk23h5+1J8k-Gqj*<-_LFbDS6P6W9_JT zR6he?{;TEa%pM`S*JqlJzo zL=g(r%2NIv#dIJ42^bF>Lm_QOAOFBPcm4Jm!~_0y3v8%9K3Y;OQDR?Qj8T&V=iIsR zE_?#5E+hausWHPP?;16MX5N7B&5@QNk%6x zqKi=B)_aIyf`h4lWqvo^cNE2*Hz%(utm(ejK3o11FH)czax9*232Df^_SsTVL+-&3 zbDke|+(OES?wizx$i*=j_u8Q&5BwOB%Z!yBjr++NK}5V0&Dhrt{cB(VJ81;2UUuJ% z9_JYDC#KH8&}yI*F6BKSlRnS3!~3gNX`=kv3znJnS-H)i8@5vC_6z0}!ayLPN)vm@sPqa__lw7GvVy=0l9@ctlPj zV%)N;eHbE_sgZHr#ZRUfCudxyi*U=&w||K(W&^@&lQU005%%dK+_Ec^G`U!HBcov9n)85n2kXWLN??CUo zI}pomW+Q_x^J1OD13hn##fFIVB2Ug5IB#xM)goPdt^0mPypit6xdFL_I&#*%c$iv2 z?yaYC*1+U2Tt37%Ib(q)hOd5b?YB1KpwX|oba2CIr$y2KA3@EgZ2MsU|I^Bb%5$v& z>%@xxsVFV~VR_Q>w1u}+n^%-=FT1+*ze?{l?J?b1@@C18ON!xp0M??Hi_R(hu<)Y7 zGYX~@6hS9RHmM&+I>^8A7vkwH1kzN#n6rI<9+o0rK;)Mbe(% zHqC6kx&G=mZv5ZxLu5z!z%9I;VHqaLK_%kh9CU1alq@j|0PRPg7(XcMNdY*zb;^1; z$$^??I#~VS6?Y@P-6k(_Xp`hb&^5T>e6ad~4a8qSwATu``Z#RfJld+{8Jcc7ShZ{& z;dXcQFc^^}IVcqLs+PHkFui2Vkja>zH+ai_wHkFk?=fn_0yl^td;W?i*P3` zh~6?H$j>cbem$a>sg<)^iAm*q=R}B~yDab$KG_85-u>N%(@==ZbRpgu8MZttml4@Ka81>G7dQUr9WRpe6tY$JL*FU*w9U_xCCDro= za0_o!O-cCrFP_ab^zC14gG=z~y32s=GT(r$k_}1n zh1_e%#?AU2i(S|y_PFpCM~{TXjhj>MaAp#=Nr0#oggg&!wyWgHL>rMC;Tby!J>kZy z2~8_Zh?6;da z$zY^ro#yHu=-9iBd|w4$ZrhWyjghxVP1WTDIXs0#$rw z9L)958JoWt+ghD-6TH)Ym=!q&gA{9Y zX>G}ll8Z}9i{}-OE{YaSFWg+Xvfu{=o2dVWPEPQrDybh3I`qPI-dAt9CmdOQC& zSBYfvCy`10kkFw`e_c5xJNGc+nbc1S9oqCxs9Htu-I|=am%or_Qa>hiXyLOTGg;AN zNnAlcCv<4xGt1hr<=23F_dT6!L^pqNg{q{U#^cc3#(mkB0tX&*Na}e!4$YnY+$I&h zLr>(o7C7=26=IVlkq5ED4h{cI`H}^&rJM1oT4CsUhLe!g15DdlXxbhc{>g6gJ#{q0 zmH;=mnkHr#6i7gT3Z?@(!?`LuHgB<~nMueS0+L@$V&-oxIPw{eI@4q6 zvsBMWjzStJ@$acUx`IhP4amW*oB7+aqlaF6QqKc&aO=%Kor5hA zJ^!Yhg*tC;{G=pF1QI7g9cEvC90#2#)JH$YnIu_46fGTgwqAq1;O6e<7tB5l1zJfT zf^nOJty9*&k)7kj8`1NJ9BiGu{4o{nhT!5{wR9f65j}y(LH^HW*-v1wZsPwJ+J;lM z8SumS7!)ulU{JuIfI$I+0tN*P3K$eHC}2>)pnySvuPOzql6uOGLoXiQO_EJ?->4v= zAwB2Dp%-7@Prjalo6axp&3U6DZ=oSz>mZtZAe|HBoC{pelPpgDaI}q&*U?a_(W2ZB z`~7lMmVB$jF<&Ga_6LG;tS)trrpM8(P^uvu?Wm&@p;e*qnviyjrX@KNZ>^*4X_umM zFu)6eU?2`pC_qq7yDuf8+U>LotIANYM_vjBLb5&pypyeBrusyP+~izNkH^K?J!!8B z2K;g?!iSDeU+sWc2hs3dN*5P(@oRi^Pf@tUMtlz_lvfagqPTA=e?7YV>yS#$H5|m*KF}?-IkO+Y& zx!~eBsK^L4JbZ0(oR|1GucD0uuA^(a*PKbw4o3@eqEuV-LBcUP>Id{gGZNuJ9`VIqKV z8-ff{`_)v;##)+MvV)ME_4HUv4VD=!qYaBSvsisfDCnL|@0=`FW0swEv#fU~!|rs* zZr0=B?0%7#I1kUV63c>T!SZ&8$Khrqf%h;DxasEIg4gd9IJcK~@o?AcVC~L-(I1h==`^lX64C|5GJgDSwex2|doo z6R|D=o~n=mks>8rgsD5BNG>#+7>)8BskWzXxTrVvZ3y;>6l}N1xH+d_xBD57M{;G}r9PA>4`s@cj;+$nGB7(2ac%nW}Yr?^?K?kOJYI3c^exWj%F7ZtX1!-;qo zp^kRn6Pp!oIpyW{q?_nW`q>Iqa?-DzKAS1MimOj%;lKgwa*901!x02GFZjI>LUafo zr`yhoV5=~UTb3M7zrg!hFZ}ZQIY#2Weh0(JymA&J!z5?yeb~w+@sv!XT;xNd96YL7 zt~}-=9w=|+ldtXdfTN-9v+b7}L6;L5bU88F9?Bm_z;20Q zbz=2g`-l;i^vi8w3C#kQDR90^mCM%EFa-Iy|idI8V*GTwc!WaWhP|RgU327BK;5^4ommq&0n&1&@YHa7;fM#> ret_service_infos; + std::vector> ret_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( + info.read_service_action_info( + ret_service_infos, ret_action_infos, recorded_bag_uri, storage_id)) << recorded_bag_uri; EXPECT_TRUE(ret_service_infos.empty()); } @@ -206,7 +208,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(); @@ -244,10 +246,12 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_services_only rosbag2_cpp::Info info; std::vector> ret_service_infos; + std::vector> ret_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( + info.read_service_action_info( + ret_service_infos, ret_action_infos, recorded_bag_uri, storage_id)) << recorded_bag_uri; ASSERT_EQ(ret_service_infos.size(), 1); EXPECT_EQ(ret_service_infos[0]->name, "/test_service"); @@ -284,7 +288,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(); @@ -331,10 +335,12 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_and_se rosbag2_cpp::Info info; std::vector> ret_service_infos; + std::vector> ret_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( + info.read_service_action_info( + ret_service_infos, ret_action_infos, 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"); 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..745b803904 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 @@ -72,13 +72,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 +91,84 @@ 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")); + output, ContainsRegex( + "Topic: /test_topic1 | Type: std_msgs/msg/String | Count: //d+ | " + "Size Contribution: //+d 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")); + output, ContainsRegex( + "Topic: /test_topic2 | Type: std_msgs/msg/String | Count: //d+ | " + "Size Contribution: //+d B | Serialization Format: cdr\n")); + + EXPECT_THAT(output, HasSubstr("Services: 2\n")); + EXPECT_THAT( - output, HasSubstr( - "Topic: /test_topic2 | Type: test_msgs/msg/Strings | Count: 1 | " - "Size Contribution: 217 B | Serialization Format: cdr\n")); + output, ContainsRegex( + "Service: /test_service1 | Type: example_interfaces/srv/AddTwoInts | Request Count: \\d+ | " + "Response Count: \\d+ | Size Contribution: \\d+ B | Serialization Format: cdr\n")); + + EXPECT_THAT( + output, ContainsRegex( + "Service: /test_service2 | Type: example_interfaces/srv/AddTwoInts | Request Count: \\d+ | " + "Response Count: \\d+ | Size Contribution: \\d+ B | Serialization Format: cdr\n")); - EXPECT_THAT(output, HasSubstr("Service: 2\n")); + EXPECT_THAT(output, HasSubstr("Actions: 2\nAction information:")); 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")); + output, ContainsRegex( + "Action: /test_action1 | Type: example_interfaces/action/Fibonacci | Topics: 2 | Service: 3 " + "| 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_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")); + output, ContainsRegex( + "Action: /test_action2 | Type: example_interfaces/action/Fibonacci | Topics: 2 | Service: 3 " + "| 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+")); } 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..34ec319587 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 actions; // action internal topics and service event topics std::vector exclude_topics; std::vector exclude_topic_types; std::vector exclude_service_events; // service event topic + std::vector exclude_actions; // action internal topics and service event topics std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; 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..d18812ccda 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,15 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) } record_options.services = service_list; + // Covert action name to related topic name + auto action_list = node.declare_parameter>( + "record.actions", std::vector()); + for (auto & action : action_list) { + auto one_action_topic_list = rosbag2_cpp::action_name_to_action_topic_name(action); + std::move(one_action_topic_list.begin(), one_action_topic_list.end(), + std::back_inserter(record_options.actions)); + } + record_options.exclude_topics = node.declare_parameter>( "record.exclude_topics", std::vector()); @@ -299,6 +310,15 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) } record_options.exclude_service_events = exclude_service_list; + // Covert action name to related topic name + auto exclude_action_list = node.declare_parameter>( + "record.exclude_actions", std::vector()); + for (auto & action : exclude_action_list) { + auto one_action_topic_list = rosbag2_cpp::action_name_to_action_topic_name(action); + std::move(one_action_topic_list.begin(), one_action_topic_list.end(), + std::back_inserter(record_options.exclude_actions)); + } + 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..189f74433d 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" @@ -127,10 +128,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_related_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 +186,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 +225,45 @@ bool TopicFilter::take_topic( return false; } } + } else { + // 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_topic_name_to_action_name(topic_name); + + if (!record_options_.all_actions) { + // Not in include action list + if (!topic_in_list(topic_name, record_options_.actions)) { + // 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 (topic_in_list(topic_name, record_options_.exclude_actions)) { + 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..8af0b45a2c 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; 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..9a5d0cd82c 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -22,12 +22,14 @@ #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_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 +64,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 +137,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 +213,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 +276,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 +358,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 +422,173 @@ 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 services_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; + + // matching service + std::string v1 = "/awesome_nice_action"; + std::string v2 = "/still_nice_action"; + + // excluded service + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // service 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 = services_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()); + + 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_THAT(recorded_messages, SizeIs(expected_messages)); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_EQ(recorded_topics.size(), 10); // One action is related to 5 topics + EXPECT_TRUE( + recorded_topics.find(v1 + "/_action/get_result/_service_event") != recorded_topics.end()); + EXPECT_TRUE( + recorded_topics.find(v2 + "/_action/get_result/_service_event") != recorded_topics.end()); +} + +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/_action/send_goal/_service_event", + "/quite_nice_namespace/but_it_is_excluded/_action/get_result/_service_event", + "/quite_nice_namespace/but_it_is_excluded/_action/cancel_goal/_service_event", + "/quite_nice_namespace/but_it_is_excluded/_action/feedback", + "/quite_nice_namespace/but_it_is_excluded/_action/status", + }; + + // matching service + std::string v1 = "/awesome_nice_action"; + std::string v2 = "/still_nice_action"; + + // excluded topics + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // service 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()); + + 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_THAT(recorded_messages, SizeIs(expected_messages)); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_EQ(recorded_topics.size(), 10); // One action is related to 5 topics + EXPECT_TRUE( + recorded_topics.find(v1 + "/_action/get_result/_service_event") != recorded_topics.end()); + EXPECT_TRUE( + recorded_topics.find(v2 + "/_action/get_result/_service_event") != recorded_topics.end()); +} 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..8b6f809f0b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -39,8 +39,47 @@ class TestTopicFilter : public Test {"/status", {"status_topic_type"}}, {"/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_topics_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,6 +240,96 @@ TEST_F(TestTopicFilter, filter_services) { } } +TEST_F(TestTopicFilter, filter_actions) { + std::map> topics_and_types{ + {"topic/a", {"type_a"}}, + // 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; + + // action name /action/a + record_options.actions = { + "/action/a/_action/send_goal/_service_event", + "/action/a/_action/get_result/_service_event", + "/action/a/_action/cancel_goal/_service_event", + "/action/a/_action/feedback", + "/action/a/_action/status", + }; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(5u, filtered_topics.size()); + for (auto & topic : record_options.services) { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()) << + "Expected topic:" << topic; + } + } + + { + rosbag2_transport::RecordOptions record_options; + record_options.actions = { + // action/a + "/action/a/_action/send_goal/_service_event", + "/action/a/_action/get_result/_service_event", + "/action/a/_action/cancel_goal/_service_event", + "/action/a/_action/feedback", + "/action/a/_action/status", + // action/b + "/action/b/_action/send_goal/_service_event", + "/action/b/_action/get_result/_service_event", + "/action/b/_action/cancel_goal/_service_event", + "/action/b/_action/feedback", + "/action/b/_action/status", + // action/d + "/action/d/_action/send_goal/_service_event", + "/action/d/_action/get_result/_service_event", + "/action/d/_action/cancel_goal/_service_event", + "/action/d/_action/feedback", + "/action/d/_action/status", + }; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(10u, filtered_topics.size()); + + std::vector expected_action_topics = { + // action/a + "/action/a/_action/send_goal/_service_event", + "/action/a/_action/get_result/_service_event", + "/action/a/_action/cancel_goal/_service_event", + "/action/a/_action/feedback", + "/action/a/_action/status", + // action/b + "/action/b/_action/send_goal/_service_event", + "/action/b/_action/get_result/_service_event", + "/action/b/_action/cancel_goal/_service_event", + "/action/b/_action/feedback", + "/action/b/_action/status", + }; + for (auto & topic : expected_action_topics) { + 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; @@ -257,7 +386,9 @@ 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_); @@ -301,13 +432,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_); - 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_topics_exist(filtered_topics, "/invalid_action"); } TEST_F(TestTopicFilter, regex_and_exclude_topics) @@ -318,10 +453,15 @@ TEST_F(TestTopicFilter, regex_and_exclude_topics) rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); - 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_topics_exist(filtered_topics, "/invalid_action"); + check_action_topics_exist(filtered_topics, "/invalidated_action"); } TEST_F(TestTopicFilter, regex_and_exclude_service_events) @@ -332,10 +472,40 @@ TEST_F(TestTopicFilter, regex_and_exclude_service_events) rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); - 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_topics_exist(filtered_topics, "/invalid_action"); + check_action_topics_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/_action/send_goal/_service_event", + "/invalidated_action/_action/get_result/_service_event", + "/invalidated_action/_action/cancel_goal/_service_event", + "/invalidated_action/_action/feedback", + "/invalidated_action/_action/status" + }; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); + + 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_topics_exist(filtered_topics, "/invalid_action"); } TEST_F(TestTopicFilter, regex_filter) @@ -345,13 +515,19 @@ TEST_F(TestTopicFilter, regex_filter) rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_with_services_); - 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_topics_exist(filtered_topics, "/invalid_action"); + check_action_topics_exist(filtered_topics, "/invalidated_action"); } TEST_F(TestTopicFilter, all_topics_overrides_regex) @@ -437,6 +613,8 @@ 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_); @@ -455,11 +633,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_); 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 +657,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); } } From 1bc1f9a217770bcdfa5416da440443634a271364 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 26 Mar 2025 15:20:26 +0800 Subject: [PATCH 02/18] Address review comment of first round Signed-off-by: Barry Xu --- ros2bag/ros2bag/api/__init__.py | 16 -- ros2bag/ros2bag/verb/info.py | 4 +- ros2bag/ros2bag/verb/record.py | 6 +- ros2bag/test/test_recorder_args_parser.py | 2 + .../include/rosbag2_cpp/action_utils.hpp | 23 +- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 7 +- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 105 ++++----- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 216 ++++++++++-------- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 18 +- .../rosbag2_cpp/writers/sequential_writer.cpp | 6 +- .../test/rosbag2_cpp/test_action_utils.cpp | 31 ++- rosbag2_py/src/rosbag2_py/_info.cpp | 24 +- .../src/rosbag2_py/format_action_info.cpp | 48 +++- .../src/rosbag2_py/format_action_info.hpp | 3 + .../src/rosbag2_py/format_bag_metadata.cpp | 29 +-- .../action_client_manager.hpp | 15 -- .../test_rosbag2_cpp_get_service_info.cpp | 55 ++--- .../test_rosbag2_info_end_to_end.cpp | 4 +- .../rosbag2_transport/record_options.hpp | 4 +- .../rosbag2_transport/topic_filter.hpp | 8 + .../config_options_from_node_params.cpp | 4 +- .../src/rosbag2_transport/topic_filter.cpp | 36 ++- .../rosbag2_transport/test_record_regex.cpp | 83 ++++--- .../rosbag2_transport/test_topic_filter.cpp | 133 +++++------ 24 files changed, 477 insertions(+), 403 deletions(-) diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index deafc3ec40..42af8c9bb3 100644 --- a/ros2bag/ros2bag/api/__init__.py +++ b/ros2bag/ros2bag/api/__init__.py @@ -321,19 +321,3 @@ def convert_service_to_service_event_topic(services): services_event_topics.append(service + '/_service_event') return services_event_topics - - -def convert_action_to_all_related_topics(actions): - action_topics = [] - - if not actions: - return action_topics - - for action in actions: - action_topics.append(action + '/_action/send_goal/_service_event') - action_topics.append(action + '/_action/get_result/_service_event') - action_topics.append(action + '/_action/cancel_goal/_service_event') - action_topics.append(action + '/_action/status') - action_topics.append(action + '/_action/feedback') - - return action_topics diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index 9f78c88aab..a5441d52cb 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -28,8 +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. Display request/response ' - 'information for action internal 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 19edb513a7..73f8b3e79c 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -18,7 +18,6 @@ from rclpy.qos import InvalidQoSProfileException from ros2bag.api import add_writer_storage_plugin_extensions -from ros2bag.api import convert_action_to_all_related_topics from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error @@ -359,8 +358,7 @@ def main(self, *, args): # noqa: D102 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) - # Covert action name to internal topic name and internal service event topic name - record_options.actions = convert_action_to_all_related_topics(args.actions) + 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 @@ -371,7 +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 = convert_action_to_all_related_topics(args.exclude_actions) + 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 f3089c7caa..1ebfba36c4 100644 --- a/ros2bag/test/test_recorder_args_parser.py +++ b/ros2bag/test/test_recorder_args_parser.py @@ -308,6 +308,8 @@ def test_recorder_validate_exclude_actions_needs_inclusive_args(test_arguments_p 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 diff --git a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp index 6e9c0d7ae6..bdf9f5681b 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp @@ -22,7 +22,7 @@ namespace rosbag2_cpp { -enum class TopicsInAction +enum class ActionInterfaceType { SendGoalEvent, CancelGoalEvent, @@ -34,26 +34,31 @@ enum class TopicsInAction ROSBAG2_CPP_PUBLIC bool -is_topic_related_to_action(const std::string & topic_name, const std::string & topic_type); +is_topic_belong_to_action(const std::string & topic_name, const std::string & topic_type); -// Call this function after is_topic_related_to_action() return true +ROSBAG2_CPP_PUBLIC +bool +is_topic_belong_to_action( + ActionInterfaceType action_interface_type, const std::string & topic_type); + +// Call this function after is_topic_belong_to_action() return true ROSBAG2_CPP_PUBLIC std::string -action_topic_name_to_action_name(const std::string & topic_name); +action_interface_name_to_action_name(const std::string & topic_name); -// Call this function after is_topic_related_to_action() return true +// Call this function after is_topic_belong_to_action() return true // Note that cancel_goal event topic and status topic return "" ROSBAG2_CPP_PUBLIC std::string -action_topic_type_to_action_type(const std::string & topic_type); +action_interface_type_to_action_type(const std::string & topic_type); ROSBAG2_CPP_PUBLIC -TopicsInAction -get_action_topic_type_from_topic_name(const std::string & topic_name); +ActionInterfaceType +get_action_interface_type(const std::string & topic_name); ROSBAG2_CPP_PUBLIC std::vector -action_name_to_action_topic_name(const std::string & action_name); +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 5202bfdc92..f72e3b30db 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -59,9 +59,10 @@ class ROSBAG2_CPP_PUBLIC Info virtual rosbag2_storage::BagMetadata read_metadata( const std::string & uri, const std::string & storage_id = ""); - virtual void read_service_action_info( - std::vector> & output_service_info, - std::vector> & output_action_info, + virtual + std::pair>, + std::vector>> + read_service_and_action_info( const std::string & uri, const std::string & storage_id = ""); virtual std::unordered_map compute_messages_size_contribution( diff --git a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp index f6ef203709..c000070e0e 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -22,60 +22,46 @@ namespace rosbag2_cpp { // The postfix of the action internal topics and service event topics -const std::unordered_map ActionTopicPostfix = { - {TopicsInAction::SendGoalEvent, "/_action/send_goal/_service_event"}, - {TopicsInAction::CancelGoalEvent, "/_action/cancel_goal/_service_event"}, - {TopicsInAction::GetResultEvent, "/_action/get_result/_service_event"}, - {TopicsInAction::Feedback, "/_action/feedback"}, - {TopicsInAction::Status, "/_action/status"} +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 ActionTopicTypeRegex = { - {TopicsInAction::SendGoalEvent, ".+/action/.+SendGoal_Event$"}, - {TopicsInAction::CancelGoalEvent, "^action_msgs/srv/CancelGoal_Event$"}, - {TopicsInAction::GetResultEvent, ".+/action/.+GetResult_Event$"}, - {TopicsInAction::Feedback, ".+/action/.+_FeedbackMessage$"}, - {TopicsInAction::Status, "^action_msgs/msg/GoalStatusArray$"} +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 = ActionTopicPostfix.at(TopicsInAction::Status).length(); +const size_t kMinActionTopicPostfixLen = + action_topic_to_postfix_map.at(ActionInterfaceType::Status).length(); -bool is_topic_related_to_action(const std::string & topic_name, const std::string & topic_type) +bool is_topic_belong_to_action(const std::string & topic_name, const std::string & topic_type) { - TopicsInAction topic = TopicsInAction::Unknown; - if (topic_name.length() <= kMinActionTopicPostfixLen) { - return false; - } else { - for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { - if (topic_name.length() > postfix.length() && - topic_name.compare( - topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) - { - topic = topic_type_enum; - break; - } - } - } - - if (topic == TopicsInAction::Unknown) { + ActionInterfaceType topic = get_action_interface_type(topic_name); + if (topic == ActionInterfaceType::Unknown) { return false; } - std::regex pattern(ActionTopicTypeRegex.at(topic)); + std::regex pattern(action_topic_type_to_regex_map.at(topic)); return std::regex_search(topic_type, pattern); } -std::string action_topic_name_to_action_name(const std::string & topic_name) +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 (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + 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) + topic_name.compare(topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) { action_name = topic_name.substr(0, topic_name.length() - postfix.length()); break; @@ -86,56 +72,65 @@ std::string action_topic_name_to_action_name(const std::string & topic_name) return action_name; } -std::string action_topic_type_to_action_type(const std::string & topic_type) +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_type_to_action_type(const std::string & topic_type) { - std::string service_type; + std::string action_type; - for (auto &[topic_type_enum, regex] : ActionTopicTypeRegex) { + 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 TopicsInAction::SendGoalEvent: + case ActionInterfaceType::SendGoalEvent: // Remove the postfix "_SendGoal_Event" - service_type = + action_type = topic_type.substr(0, topic_type.length() - std::strlen("_SendGoal_Event")); break; - case TopicsInAction::GetResultEvent: + case ActionInterfaceType::GetResultEvent: // Remove the postfix "_GetResult_Event" - service_type = + action_type = topic_type.substr(0, topic_type.length() - std::strlen("_GetResult_Event")); break; - case TopicsInAction::Feedback: + case ActionInterfaceType::Feedback: // Remove the postfix "_FeedbackMessage" - service_type = + action_type = topic_type.substr(0, topic_type.length() - std::strlen("_FeedbackMessage")); break; - case TopicsInAction::CancelGoalEvent: - case TopicsInAction::Status: + case ActionInterfaceType::CancelGoalEvent: + case ActionInterfaceType::Status: default: break; } - return service_type; + return action_type; } } - return service_type; + return action_type; } -TopicsInAction get_action_topic_type_from_topic_name(const std::string & topic_name) +ActionInterfaceType get_action_interface_type(const std::string & topic_name) { - for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + 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) + topic_name.compare(topic_name.length() - postfix.length(), postfix.length(), postfix) == 0) { return topic_type_enum; } } - return TopicsInAction::Unknown; + return ActionInterfaceType::Unknown; } -std::vector action_name_to_action_topic_name(const std::string & action_name) +std::vector action_name_to_action_interface_names(const std::string & action_name) { std::vector action_topics; @@ -143,7 +138,7 @@ std::vector action_name_to_action_topic_name(const std::string & ac return action_topics; } - for (auto &[topic_type_enum, postfix] : ActionTopicPostfix) { + for (auto &[topic_type_enum, postfix] : action_topic_to_postfix_map) { action_topics.push_back(action_name + postfix); } diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 598cb05b72..8c69050a0d 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -76,11 +76,81 @@ struct action_service_req_resp_info service_req_resp_info cancel_goal_service; service_req_resp_info get_result_service; }; + +inline void calculate_message_counts( + std::unordered_map & message_map, + size_t & count) +{ + count = 0; + for (auto & [client_id, message_list] : message_map) { + count += message_list.size(); + } +} + +using action_analysis = + std::unordered_map>; + +inline void summary_action_service_info( + action_analysis & action_process_info, + std::unordered_map> & all_action_info) +{ + for (auto & [action_name, action_info] : action_process_info) { + size_t count = 0; + // Get the number of request from all clients for send_goal + calculate_message_counts(action_info->send_goal_service.request, count); + all_action_info[action_name]->send_goal_service_msg_count.first = count; + + // Get the number of request from all clients for cancel_goal + count = 0; + calculate_message_counts(action_info->cancel_goal_service.request, count); + all_action_info[action_name]->cancel_goal_service_msg_count.first = count; + + // Get the number of request from all clients for get_result + count = 0; + calculate_message_counts(action_info->get_result_service.request, count); + all_action_info[action_name]->get_result_service_msg_count.first = count; + + // Get the number of response from all clients for send_goal + count = 0; + calculate_message_counts(action_info->send_goal_service.response, count); + all_action_info[action_name]->send_goal_service_msg_count.second = count; + + // Get the number of response from all clients for cancel_goal + count = 0; + calculate_message_counts(action_info->cancel_goal_service.response, count); + all_action_info[action_name]->cancel_goal_service_msg_count.second = count; + + // Get the number of response from all clients for get_result + count = 0; + calculate_message_counts(action_info->get_result_service.response, count); + all_action_info[action_name]->get_result_service_msg_count.second = count; + } +} + +using service_analysis = + std::unordered_map>; + +inline void summary_service_info( + service_analysis & service_process_info, + std::unordered_map> & all_service_info) +{ + for (auto & [topic_name, service_info] : service_process_info) { + size_t count = 0; + // Get the number of request from all clients + calculate_message_counts(service_info->request, count); + all_service_info[topic_name]->request_count = count; + + count = 0; + // Get the number of response from all clients + calculate_message_counts(service_info->response, count); + all_service_info[topic_name]->response_count = count; + } +} } // namespace -void Info::read_service_action_info( - std::vector> & output_service_info, - std::vector> & output_action_info, +std::pair>, + std::vector>> +Info::read_service_and_action_info( const std::string & uri, const std::string & storage_id) { rosbag2_storage::StorageFactory factory; @@ -95,22 +165,24 @@ void Info::read_service_action_info( } // Service event topic name as key - using service_analysis = - std::unordered_map>; service_analysis service_process_info; std::unordered_map> all_service_info; // Action name as key - using action_analysis = - std::unordered_map>; 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_topic_related_to_action(t.name, t.type)) { + if (is_topic_belong_to_action(t.name, t.type)) { std::shared_ptr action_info; - std::string action_name = action_topic_name_to_action_name(t.name); + 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; @@ -121,20 +193,27 @@ void Info::read_service_action_info( action_info = all_action_info[action_name]; } - // cancel_goal event topic and status topic cannot get type + // Update action type. Note: cancel_goal event topic and status topic cannot get type if (action_info->type.empty()) { - action_info->type = action_topic_type_to_action_type(t.type); + action_info->type = action_interface_type_to_action_type(t.type); } - continue; - } - if (is_service_event_topic(t.name, t.type)) { + // Update action_interface_name_to_action_name_map to speed up following code. + if (action_interface_name_to_action_name_map.find(t.name) == + action_interface_name_to_action_name_map.end()) + { + 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; } } @@ -147,17 +226,15 @@ void Info::read_service_action_info( while (storage->has_next()) { auto bag_msg = storage->read_next(); - auto action_topic_type = get_action_topic_type_from_topic_name(bag_msg->topic_name); - if (action_topic_type == TopicsInAction::Unknown) { - // 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()) { - continue; // Skip the regular topics - } + 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 } - if (action_topic_type == TopicsInAction::Feedback || - action_topic_type == TopicsInAction::Status) + auto action_interface_type = get_action_interface_type(bag_msg->topic_name); + if (action_interface_type == ActionInterfaceType::Feedback || + action_interface_type == ActionInterfaceType::Status) { continue; // Skip the feedback and status topic for action } @@ -171,23 +248,24 @@ void Info::read_service_action_info( "Failed to deserialize message from " + bag_msg->topic_name + " !"); } - if (action_topic_type != TopicsInAction::Unknown) { - auto action_name = action_topic_name_to_action_name(bag_msg->topic_name); + if (action_interface_type != ActionInterfaceType::Unknown) { + auto action_name = action_interface_name_to_action_name_map[bag_msg->topic_name]; + auto action_service_info = 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_topic_type == TopicsInAction::SendGoalEvent) { - action_process_info[action_name]-> + if (action_interface_type == ActionInterfaceType::SendGoalEvent) { + action_service_info-> send_goal_service.request[msg.client_gid].emplace(msg.sequence_number); - } else if (action_topic_type == TopicsInAction::GetResultEvent) { - action_process_info[action_name]-> + } else if (action_interface_type == ActionInterfaceType::GetResultEvent) { + action_service_info-> get_result_service.request[msg.client_gid].emplace(msg.sequence_number); } else { // TopicsInAction::CancelGoalEvent - action_process_info[action_name]-> + action_service_info-> cancel_goal_service.request[msg.client_gid].emplace(msg.sequence_number); } break; @@ -195,19 +273,22 @@ void Info::read_service_action_info( case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: { - if (action_topic_type == TopicsInAction::SendGoalEvent) { - action_process_info[action_name]-> + if (action_interface_type == ActionInterfaceType::SendGoalEvent) { + action_service_info-> send_goal_service.response[msg.client_gid].emplace(msg.sequence_number); - } else if (action_topic_type == TopicsInAction::GetResultEvent) { - action_process_info[action_name]-> + } else if (action_interface_type == ActionInterfaceType::GetResultEvent) { + action_service_info-> get_result_service.response[msg.client_gid].emplace(msg.sequence_number); } else { // TopicsInAction::CancelGoalEvent - action_process_info[action_name]-> + action_service_info-> 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 @@ -222,54 +303,15 @@ void Info::read_service_action_info( service_process_info[bag_msg->topic_name]->response[msg.client_gid].emplace( msg.sequence_number); break; + default: + throw std::range_error("Invalid service event type " + + std::to_string(msg.event_type) + " !"); } } } // Process action_process_info to get the number of request and response - for (auto & [action_name, action_info] : action_process_info) { - size_t count = 0; - // Get the number of request from all clients for send_goal - for (auto &[client_id, request_list] : action_info->send_goal_service.request) { - count += request_list.size(); - } - all_action_info[action_name]->send_goal_service_msg_count.first = count; - - // Get the number of request from all clients for cancel_goal - count = 0; - for (auto &[client_id, request_list] : action_info->cancel_goal_service.request) { - count += request_list.size(); - } - all_action_info[action_name]->cancel_goal_service_msg_count.first = count; - - // Get the number of request from all clients for get_result - count = 0; - for (auto &[client_id, request_list] : action_info->get_result_service.request) { - count += request_list.size(); - } - all_action_info[action_name]->get_result_service_msg_count.first = count; - - // Get the number of response from all clients for send_goal - count = 0; - for (auto &[client_id, response_list] : action_info->send_goal_service.response) { - count += response_list.size(); - } - all_action_info[action_name]->send_goal_service_msg_count.second = count; - - // Get the number of response from all clients for cancel_goal - count = 0; - for (auto &[client_id, response_list] : action_info->cancel_goal_service.response) { - count += response_list.size(); - } - all_action_info[action_name]->cancel_goal_service_msg_count.second = count; - - // Get the number of response from all clients for get_result - count = 0; - for (auto &[client_id, response_list] : action_info->get_result_service.response) { - count += response_list.size(); - } - all_action_info[action_name]->get_result_service_msg_count.second = count; - } + summary_action_service_info(action_process_info, all_action_info); // Covert all_action_info to output_action_info for (auto & [action_name, action_info] : all_action_info) { @@ -277,21 +319,7 @@ void Info::read_service_action_info( } // Process service_process_info to get the number of request and response - 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; - - 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; - } + summary_service_info(service_process_info, all_service_info); // Convert all_service_info to output_service_info for (auto & [topic_name, service_info] : all_service_info) { @@ -299,7 +327,7 @@ void Info::read_service_action_info( } } - return; + 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 ae152ab0ad..aee9d4dd09 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -28,20 +28,12 @@ const size_t kServiceEventTypePostfixLen = strlen(kServiceEventTopicTypePostfix) bool is_service_event_topic(const std::string & topic_name, const std::string & topic_type) { - if (topic_name.length() <= kServiceEventTopicPostfixLen) { + if ((topic_name.length() <= kServiceEventTopicPostfixLen) || + (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) || + (topic_name.find("/_action/") != std::string::npos)) + { return false; - } else { - // Not services for action - if (topic_name.find("/_action/") != std::string::npos) { - return false; - } - - // 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_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 5c444c07b3..657b9212e8 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -225,12 +225,14 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic rosbag2_storage::MessageDefinition definition; std::string topic_type; - if (is_topic_related_to_action(topic_with_type.name, topic_with_type.type)) { + if (is_topic_belong_to_action(topic_with_type.name, topic_with_type.type)) { // The following two action topic types cannot retrieve the action type. // - xxx/_action/cancel_goal/_service_event (action_msgs/srv/CancelGoal_Event) // - xxx/_action/status (action_msgs/msg/GoalStatusArray) - topic_type = action_topic_type_to_action_type(topic_with_type.type); + topic_type = action_interface_type_to_action_type(topic_with_type.type); + // TODO(Barry.Xu): get_full_text() not support action type. Need to implement it. + // Now action type return empty message definition. definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_type); } else { if (is_service_event_topic(topic_with_type.name, topic_with_type.type)) { diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp index a3d6cb03b0..8ece07c36e 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp @@ -62,7 +62,7 @@ TEST_F(ActionUtilsTest, check_is_topic_related_to_action) for (const auto & test_data : all_test_data) { EXPECT_TRUE( - rosbag2_cpp::is_topic_related_to_action( + rosbag2_cpp::is_topic_belong_to_action( std::get<0>(test_data.first), std::get<1>(test_data.first)) == test_data.second); } } @@ -82,7 +82,7 @@ TEST_F(ActionUtilsTest, check_action_topic_name_to_action_name) for (const auto & test_data : all_test_data) { EXPECT_TRUE( - rosbag2_cpp::action_topic_name_to_action_name(test_data.first) == test_data.second); + rosbag2_cpp::action_interface_name_to_action_name(test_data.first) == test_data.second); } } @@ -99,30 +99,27 @@ TEST_F(ActionUtilsTest, check_action_topic_type_to_action_type) for (const auto & test_data : all_test_data) { EXPECT_EQ( - rosbag2_cpp::action_topic_type_to_action_type(test_data.first), + rosbag2_cpp::action_interface_type_to_action_type(test_data.first), test_data.second ); } } -TEST_F(ActionUtilsTest, check_get_action_topic_type_from_topic_name) +TEST_F(ActionUtilsTest, check_get_action_interface_type) { - std::vector> all_test_data = + std::vector> all_test_data = { - {"/abc/_action/feedback", rosbag2_cpp::TopicsInAction::Feedback}, - {"/abc/_action/get_result/_service_event", rosbag2_cpp::TopicsInAction::GetResultEvent}, - {"/abc/_action/send_goal/_service_event", rosbag2_cpp::TopicsInAction::SendGoalEvent}, - {"/abc/_action/cancel_goal/_service_event", rosbag2_cpp::TopicsInAction::CancelGoalEvent}, - {"/abc/_action/status", rosbag2_cpp::TopicsInAction::Status}, - {"/_action/status", rosbag2_cpp::TopicsInAction::Unknown}, - {"/abc/action/status", rosbag2_cpp::TopicsInAction::Unknown} + {"/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 & test_data : all_test_data) { - EXPECT_EQ( - rosbag2_cpp::get_action_topic_type_from_topic_name(test_data.first), - test_data.second - ); + EXPECT_EQ(rosbag2_cpp::get_action_interface_type(test_data.first), test_data.second); } } @@ -138,7 +135,7 @@ TEST_F(ActionUtilsTest, check_action_name_to_action_topic_name) "abc/_action/status" }; - auto output_action_topics = rosbag2_cpp::action_name_to_action_topic_name(action_name); + auto output_action_topics = rosbag2_cpp::action_name_to_action_interface_names(action_name); for (auto & topic : output_action_topics) { EXPECT_TRUE(std::find( diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 7ab08cd310..5c13d7668e 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -67,7 +67,7 @@ class Info if (!rosbag2_cpp::is_service_event_topic( topic_info.topic_metadata.name, topic_info.topic_metadata.type) && - !rosbag2_cpp::is_topic_related_to_action( + !rosbag2_cpp::is_topic_belong_to_action( topic_info.topic_metadata.name, topic_info.topic_metadata.type)) { @@ -85,11 +85,7 @@ class Info std::vector> all_actions_info; for (auto & file_info : metadata_info.files) { - std::vector> output_service_info; - std::vector> output_action_info; - info_->read_service_action_info( - output_service_info, - output_action_info, + auto [output_service_info, output_action_info] = info_->read_service_and_action_info( uri + "/" + file_info.path, metadata_info.storage_identifier); if (!output_service_info.empty()) { @@ -110,24 +106,24 @@ class Info // metadata_info.topics_with_message_count if (!all_actions_info.empty()) { for (auto & [topic_metadata, message_count] : metadata_info.topics_with_message_count) { - if (rosbag2_cpp::is_topic_related_to_action(topic_metadata.name, topic_metadata.type)) { - auto action_interface_type = - rosbag2_cpp::get_action_topic_type_from_topic_name(topic_metadata.name); + auto action_interface_type = + rosbag2_cpp::get_action_interface_type(topic_metadata.name); + if (rosbag2_cpp::is_topic_belong_to_action(action_interface_type, topic_metadata.type)) { switch (action_interface_type) { - case rosbag2_cpp::TopicsInAction::Feedback: - case rosbag2_cpp::TopicsInAction::Status: + case rosbag2_cpp::ActionInterfaceType::Feedback: + case rosbag2_cpp::ActionInterfaceType::Status: { auto action_info_iter = std::find_if(all_actions_info.begin(), all_actions_info.end(), [topic_name = topic_metadata.name](const auto & action_info){ return action_info->name == - rosbag2_cpp::action_topic_name_to_action_name(topic_name); + rosbag2_cpp::action_interface_name_to_action_name(topic_name); }); if (action_info_iter == all_actions_info.end()) { break; } - if (action_interface_type == rosbag2_cpp::TopicsInAction::Feedback) { + if (action_interface_type == rosbag2_cpp::ActionInterfaceType::Feedback) { (*action_info_iter)->feedback_topic_msg_count = message_count; } else { (*action_info_iter)->status_topic_msg_count = message_count; @@ -157,7 +153,7 @@ class Info std::cout << format_service_info(all_services_info, messages_size, true, sort_method) << std::endl; std::cout << - format_action_info(all_actions_info, sort_method) << std::endl; + 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/format_action_info.cpp b/rosbag2_py/src/rosbag2_py/format_action_info.cpp index 5dc6c34cb4..926b17c998 100644 --- a/rosbag2_py/src/rosbag2_py/format_action_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_action_info.cpp @@ -13,16 +13,40 @@ // limitations under the License. #include +#include #include "format_action_info.hpp" #include "rosbag2_cpp/action_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 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; @@ -34,13 +58,31 @@ format_action_info( 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](const std::shared_ptr & ai) -> void { + [&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; @@ -60,8 +102,8 @@ format_action_info( std::vector sorted_idx = generate_sorted_idx(action_info_list, sort_method); print_action_info(action_info_list[sorted_idx[0]]); - auto number_of_services = action_info_list.size(); - for (size_t j = 1; j < number_of_services; ++j) { + auto number_of_actions = action_info_list.size(); + for (size_t j = 1; j < number_of_actions; ++j) { print_action_info(action_info_list[sorted_idx[j]]); } diff --git a/rosbag2_py/src/rosbag2_py/format_action_info.hpp b/rosbag2_py/src/rosbag2_py/format_action_info.hpp index 5639937390..0844b9adc8 100644 --- a/rosbag2_py/src/rosbag2_py/format_action_info.hpp +++ b/rosbag2_py/src/rosbag2_py/format_action_info.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "info_sorting_method.hpp" @@ -27,6 +28,8 @@ 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 diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index e807c2af5d..1e61d7fa0a 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -152,7 +152,7 @@ void format_topics_with_type( (rosbag2_cpp::is_service_event_topic( topics[sorted_idx[i]].topic_metadata.name, topics[sorted_idx[i]].topic_metadata.type) || - rosbag2_cpp::is_topic_related_to_action( + rosbag2_cpp::is_topic_belong_to_action( topics[sorted_idx[i]].topic_metadata.name, topics[sorted_idx[i]].topic_metadata.type))) { @@ -168,7 +168,7 @@ void format_topics_with_type( 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_related_to_action( + rosbag2_cpp::is_topic_belong_to_action( topics[sorted_idx[j]].topic_metadata.name, topics[sorted_idx[j]].topic_metadata.type)) { continue; @@ -196,10 +196,11 @@ void filter_service_and_action_info( ActionInfoMap action_info_map; for (auto & topic : topics_with_message_count) { - if (rosbag2_cpp::is_topic_related_to_action( - topic.topic_metadata.name, topic.topic_metadata.type)) - { - auto action_name = rosbag2_cpp::action_topic_name_to_action_name(topic.topic_metadata.name); + 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(); @@ -212,27 +213,27 @@ void filter_service_and_action_info( // 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::action_topic_type_to_action_type(topic.topic_metadata.type); + rosbag2_cpp::action_interface_type_to_action_type(topic.topic_metadata.type); } - switch (rosbag2_cpp::get_action_topic_type_from_topic_name(topic.topic_metadata.name)) { - case rosbag2_cpp::TopicsInAction::SendGoalEvent: + 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::TopicsInAction::CancelGoalEvent: + case rosbag2_cpp::ActionInterfaceType::CancelGoalEvent: action_info_map[action_name]->cancel_goal_event_message_count = topic.message_count; break; - case rosbag2_cpp::TopicsInAction::GetResultEvent: + case rosbag2_cpp::ActionInterfaceType::GetResultEvent: action_info_map[action_name]->get_result_event_message_count = topic.message_count; break; - case rosbag2_cpp::TopicsInAction::Feedback: + case rosbag2_cpp::ActionInterfaceType::Feedback: action_info_map[action_name]->feedback_message_count = topic.message_count; break; - case rosbag2_cpp::TopicsInAction::Status: + case rosbag2_cpp::ActionInterfaceType::Status: action_info_map[action_name]->status_message_count = topic.message_count; break; default: // Never go here - break; + throw std::out_of_range("Invalid action interface type"); } total_action_msg_count += topic.message_count; continue; 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 index e1055defae..3f5912046a 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp @@ -216,30 +216,15 @@ class ActionClientManager : public rclcpp::Node rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); - auto & sequence = feedback->sequence; - sequence.push_back(0); - sequence.push_back(1); auto result = std::make_shared(); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { - // Check if there is a cancel request - if (goal_handle->is_canceling()) { - result->sequence = sequence; - goal_handle->canceled(result); - return; - } - // Update sequence - sequence.push_back(sequence[i] + sequence[i - 1]); - // Publish feedback goal_handle->publish_feedback(feedback); - - // loop_rate.sleep(); } loop_rate.sleep(); // goal is done - result->sequence = sequence; goal_handle->succeed(result); } }; 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 5abe071309..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,14 +182,15 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_only) } rosbag2_cpp::Info info; - std::vector> ret_service_infos; - std::vector> ret_action_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( - info.read_service_action_info( - ret_service_infos, ret_action_infos, recorded_bag_uri, storage_id)) << recorded_bag_uri; + 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) { @@ -245,20 +246,21 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_services_only } rosbag2_cpp::Info info; - std::vector> ret_service_infos; - std::vector> ret_action_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( - info.read_service_action_info( - ret_service_infos, ret_action_infos, 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"); + 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) { @@ -334,21 +336,22 @@ TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_and_se } rosbag2_cpp::Info info; - std::vector> ret_service_infos; - std::vector> ret_action_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( - info.read_service_action_info( - ret_service_infos, ret_action_infos, 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"); + 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 745b803904..642f16ed1b 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 @@ -153,7 +153,7 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { EXPECT_THAT( output, ContainsRegex( "Action: /test_action1 | Type: example_interfaces/action/Fibonacci | Topics: 2 | Service: 3 " - "| Serialization Format: cdr\n" + "| 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" @@ -163,7 +163,7 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { EXPECT_THAT( output, ContainsRegex( "Action: /test_action2 | Type: example_interfaces/action/Fibonacci | Topics: 2 | Service: 3 " - "| Serialization Format: cdr\n" + "| 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" diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index 34ec319587..f31a55a45b 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -37,11 +37,11 @@ struct RecordOptions std::vector topics; std::vector topic_types; std::vector services; // service event topic - std::vector actions; // action internal topics and service event topics + std::vector actions; // action name list std::vector exclude_topics; std::vector exclude_topic_types; std::vector exclude_service_events; // service event topic - std::vector exclude_actions; // action internal topics and service event topics + std::vector exclude_actions; // action 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 d18812ccda..61de50ed95 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 @@ -291,7 +291,7 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) auto action_list = node.declare_parameter>( "record.actions", std::vector()); for (auto & action : action_list) { - auto one_action_topic_list = rosbag2_cpp::action_name_to_action_topic_name(action); + auto one_action_topic_list = rosbag2_cpp::action_name_to_action_interface_names(action); std::move(one_action_topic_list.begin(), one_action_topic_list.end(), std::back_inserter(record_options.actions)); } @@ -314,7 +314,7 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) auto exclude_action_list = node.declare_parameter>( "record.exclude_actions", std::vector()); for (auto & action : exclude_action_list) { - auto one_action_topic_list = rosbag2_cpp::action_name_to_action_topic_name(action); + auto one_action_topic_list = rosbag2_cpp::action_name_to_action_interface_names(action); std::move(one_action_topic_list.begin(), one_action_topic_list.end(), std::back_inserter(record_options.exclude_actions)); } diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index 189f74433d..52489320d1 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -104,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; @@ -129,7 +147,7 @@ bool TopicFilter::take_topic( const std::string & topic_type = topic_types[0]; - bool is_action_topic = rosbag2_cpp::is_topic_related_to_action(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); @@ -225,7 +243,7 @@ bool TopicFilter::take_topic( return false; } } - } else { + } else if (is_action_topic) { // action topic // Check if topics for action need to be recorded @@ -237,11 +255,13 @@ bool TopicFilter::take_topic( } // Convert topic name to action name - auto action_name = rosbag2_cpp::action_topic_name_to_action_name(topic_name); + auto action_name = rosbag2_cpp::action_interface_name_to_action_name(topic_name); if (!record_options_.all_actions) { - // Not in include action list - if (!topic_in_list(topic_name, record_options_.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); @@ -254,7 +274,9 @@ bool TopicFilter::take_topic( } } - if (topic_in_list(topic_name, record_options_.exclude_actions)) { + if (exclude_action_interface_names_.find(topic_name) != + exclude_action_interface_names_.end()) + { return false; } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 9a5d0cd82c..3a3f94e4c0 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -27,6 +27,7 @@ #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_transport/recorder.hpp" #include "test_msgs/action/fibonacci.hpp" @@ -426,23 +427,23 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_action_recording) { std::string regex = "/[a-z]+_nice(_.*)"; - std::string services_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; + std::string actions_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; - // matching service + // matching action std::string v1 = "/awesome_nice_action"; std::string v2 = "/still_nice_action"; - // excluded service + // excluded action std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; - // service that shouldn't match + // 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 = services_regex_to_exclude; + record_options.exclude_regex = actions_regex_to_exclude; auto action_manager_v1 = std::make_shared>(v1); @@ -487,43 +488,55 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_action_recording) ASSERT_TRUE(action_manager_b1->send_goal()); ASSERT_TRUE(action_manager_b2->send_goal()); - constexpr size_t expected_messages = 16; + // One action include at least 8 messages + // Goal request received, Goal response sent, 3 feedback, Result request received + // Result response sent, 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_messages; + 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_messages)); + EXPECT_THAT(recorded_messages, SizeIs(Ge(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 - EXPECT_TRUE( - recorded_topics.find(v1 + "/_action/get_result/_service_event") != recorded_topics.end()); - EXPECT_TRUE( - recorded_topics.find(v2 + "/_action/get_result/_service_event") != recorded_topics.end()); + + 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/_action/send_goal/_service_event", - "/quite_nice_namespace/but_it_is_excluded/_action/get_result/_service_event", - "/quite_nice_namespace/but_it_is_excluded/_action/cancel_goal/_service_event", - "/quite_nice_namespace/but_it_is_excluded/_action/feedback", - "/quite_nice_namespace/but_it_is_excluded/_action/status", + "/quite_nice_namespace/but_it_is_excluded", }; - // matching service + // matching actions std::string v1 = "/awesome_nice_action"; std::string v2 = "/still_nice_action"; - // excluded topics + // excluded action std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; - // service that shouldn't match + // action that shouldn't match std::string b1 = "/numberslike1arenot_nice"; std::string b2 = "/namespace_before/not_nice"; @@ -575,20 +588,36 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_actions_action_recording) ASSERT_TRUE(action_manager_b1->send_goal()); ASSERT_TRUE(action_manager_b2->send_goal()); - constexpr size_t expected_messages = 16; + // One action include at least 8 messages + // Goal request received, Goal response sent, 3 feedback, Result request received + // Result response sent, 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_messages; + 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_messages)); + EXPECT_THAT(recorded_messages, SizeIs(Ge(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 - EXPECT_TRUE( - recorded_topics.find(v1 + "/_action/get_result/_service_event") != recorded_topics.end()); - EXPECT_TRUE( - recorded_topics.find(v2 + "/_action/get_result/_service_event") != recorded_topics.end()); + + 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_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index 8b6f809f0b..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,6 +38,7 @@ 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"}}, @@ -67,7 +69,7 @@ class TestTopicFilter : public Test {"/planning_action/_action/status", {"action_msgs/msg/GoalStatusArray"}}, }; - void check_action_topics_exist( + void check_action_interfaces_exist( std::unordered_map & filtered_topics, const std::string action_name) { @@ -243,6 +245,7 @@ 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"}}, @@ -266,18 +269,15 @@ TEST_F(TestTopicFilter, filter_actions) { { rosbag2_transport::RecordOptions record_options; - // action name /action/a record_options.actions = { - "/action/a/_action/send_goal/_service_event", - "/action/a/_action/get_result/_service_event", - "/action/a/_action/cancel_goal/_service_event", - "/action/a/_action/feedback", - "/action/a/_action/status", + "/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()); - for (auto & topic : record_options.services) { + 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; } @@ -286,44 +286,29 @@ TEST_F(TestTopicFilter, filter_actions) { { rosbag2_transport::RecordOptions record_options; record_options.actions = { - // action/a - "/action/a/_action/send_goal/_service_event", - "/action/a/_action/get_result/_service_event", - "/action/a/_action/cancel_goal/_service_event", - "/action/a/_action/feedback", - "/action/a/_action/status", - // action/b - "/action/b/_action/send_goal/_service_event", - "/action/b/_action/get_result/_service_event", - "/action/b/_action/cancel_goal/_service_event", - "/action/b/_action/feedback", - "/action/b/_action/status", - // action/d - "/action/d/_action/send_goal/_service_event", - "/action/d/_action/get_result/_service_event", - "/action/d/_action/cancel_goal/_service_event", - "/action/d/_action/feedback", - "/action/d/_action/status", + "/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()); - std::vector expected_action_topics = { - // action/a - "/action/a/_action/send_goal/_service_event", - "/action/a/_action/get_result/_service_event", - "/action/a/_action/cancel_goal/_service_event", - "/action/a/_action/feedback", - "/action/a/_action/status", - // action/b - "/action/b/_action/send_goal/_service_event", - "/action/b/_action/get_result/_service_event", - "/action/b/_action/cancel_goal/_service_event", - "/action/b/_action/feedback", - "/action/b/_action/status", - }; - for (auto & topic : expected_action_topics) { + for (auto & topic : expected_action_interfaces_name) { EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()) << "Expected topic:" << topic; } @@ -336,7 +321,7 @@ TEST_F(TestTopicFilter, all_topics_and_exclude_regex) 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"}) { @@ -353,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"}) { @@ -369,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 : @@ -390,7 +375,7 @@ TEST_F(TestTopicFilter, all_services_and_exclude_regex) 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); @@ -405,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); @@ -418,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 : @@ -434,7 +419,7 @@ TEST_F(TestTopicFilter, regex_and_exclude_regex) record_options.regex = "/invalid.*"; 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(7)); // Matched topic @@ -442,7 +427,7 @@ TEST_F(TestTopicFilter, regex_and_exclude_regex) // Matched service EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); // Matched action - check_action_topics_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalid_action"); } TEST_F(TestTopicFilter, regex_and_exclude_topics) @@ -451,7 +436,7 @@ 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(13)); // Matched topic @@ -460,8 +445,8 @@ TEST_F(TestTopicFilter, regex_and_exclude_topics) 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_topics_exist(filtered_topics, "/invalid_action"); - check_action_topics_exist(filtered_topics, "/invalidated_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) @@ -470,7 +455,7 @@ 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(13)); // Matched topic @@ -479,8 +464,8 @@ TEST_F(TestTopicFilter, regex_and_exclude_service_events) // Matched service EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); // Matched action - check_action_topics_exist(filtered_topics, "/invalid_action"); - check_action_topics_exist(filtered_topics, "/invalidated_action"); + check_action_interfaces_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalidated_action"); } TEST_F(TestTopicFilter, regex_and_exclude_actions) @@ -488,14 +473,10 @@ TEST_F(TestTopicFilter, regex_and_exclude_actions) rosbag2_transport::RecordOptions record_options; record_options.regex = "/invalid.*"; record_options.exclude_actions = { - "/invalidated_action/_action/send_goal/_service_event", - "/invalidated_action/_action/get_result/_service_event", - "/invalidated_action/_action/cancel_goal/_service_event", - "/invalidated_action/_action/feedback", - "/invalidated_action/_action/status" + "/invalidated_action" }; 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(9)); // Matched topic @@ -505,7 +486,7 @@ TEST_F(TestTopicFilter, regex_and_exclude_actions) 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_topics_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalid_action"); } TEST_F(TestTopicFilter, regex_filter) @@ -513,7 +494,7 @@ 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(14)); @@ -526,8 +507,8 @@ TEST_F(TestTopicFilter, regex_filter) } // Matched action - check_action_topics_exist(filtered_topics, "/invalid_action"); - check_action_topics_exist(filtered_topics, "/invalidated_action"); + check_action_interfaces_exist(filtered_topics, "/invalid_action"); + check_action_interfaces_exist(filtered_topics, "/invalidated_action"); } TEST_F(TestTopicFilter, all_topics_overrides_regex) @@ -536,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)); } @@ -546,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()); @@ -560,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()); @@ -576,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()); @@ -589,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)); } @@ -600,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()); @@ -617,7 +598,7 @@ TEST_F(TestTopicFilter, do_not_print_warning_about_unknown_types_if_topic_is_not 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( @@ -637,7 +618,7 @@ TEST_F(TestTopicFilter, do_not_print_warning_about_unknown_types_if_topic_is_not 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(2u, filtered_topics.size()); @@ -679,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)); } @@ -690,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)); } From 6c437c069eb0189259225511d95c149d23669424 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 26 Mar 2025 18:11:05 +0800 Subject: [PATCH 03/18] Address review comments from fujitatomoya Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 2 +- .../src/rosbag2_py/format_action_info.cpp | 3 +- .../src/rosbag2_py/format_bag_metadata.cpp | 2 +- .../action_client_manager.hpp | 35 +++++++++---- .../config_options_from_node_params.cpp | 16 +----- .../rosbag2_transport/test_record_all.cpp | 49 +++++++++++++++++++ .../rosbag2_transport/test_record_regex.cpp | 12 ++--- 7 files changed, 85 insertions(+), 34 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 73f8b3e79c..195f3ceaf8 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -301,7 +301,7 @@ 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 '' + return None class RecordVerb(VerbExtension): diff --git a/rosbag2_py/src/rosbag2_py/format_action_info.cpp b/rosbag2_py/src/rosbag2_py/format_action_info.cpp index 926b17c998..44da48237d 100644 --- a/rosbag2_py/src/rosbag2_py/format_action_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_action_info.cpp @@ -101,9 +101,8 @@ format_action_info( std::vector sorted_idx = generate_sorted_idx(action_info_list, sort_method); - print_action_info(action_info_list[sorted_idx[0]]); auto number_of_actions = action_info_list.size(); - for (size_t j = 1; j < number_of_actions; ++j) { + for (size_t j = 0; j < number_of_actions; ++j) { print_action_info(action_info_list[sorted_idx[j]]); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index 1e61d7fa0a..b316352af6 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -147,7 +147,7 @@ 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( topics[sorted_idx[i]].topic_metadata.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 index 3f5912046a..b135b53556 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/action_client_manager.hpp @@ -42,6 +42,7 @@ class ActionClientManager : public rclcpp::Node 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) @@ -53,7 +54,7 @@ class ActionClientManager : public rclcpp::Node enable_action_server_introspection_(enable_action_server_introspection), enable_action_client_introspection_(enable_action_client_introspection) { - create_action_server(); + create_action_server(exec_goal_time); create_action_client(number_of_clients); @@ -78,7 +79,7 @@ class ActionClientManager : public rclcpp::Node } } - void create_action_server() + void create_action_server(std::chrono::seconds exec_goal_time) { auto handle_goal = [this]( const rclcpp_action::GoalUUID & uuid, @@ -95,12 +96,15 @@ class ActionClientManager : public rclcpp::Node return rclcpp_action::CancelResponse::ACCEPT; }; - auto handle_accepted = [this]( + 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]() {return this->execute(goal_handle);}; + auto execute_in_thread = + [this, goal_handle, exec_goal_time]() { + return this->execute(goal_handle, exec_goal_time); + }; std::thread{execute_in_thread}.detach(); }; @@ -147,7 +151,9 @@ class ActionClientManager : public rclcpp::Node return check_action_server_ready(); } - bool send_goal(std::chrono::duration timeout = std::chrono::seconds(10)) + bool send_goal( + bool cancel_goal_after_accept = false, + std::chrono::duration timeout = std::chrono::seconds(10)) { if (!check_action_server_ready()) { return false; @@ -158,10 +164,14 @@ class ActionClientManager : public rclcpp::Node goal_msg.order = 3; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = [this]( + send_goal_options.goal_response_callback = [this, action_client, cancel_goal_after_accept]( const ClientGoalHandleFibonacci::SharedPtr & goal_handle) { - (void)goal_handle; + if (goal_handle) { + if (cancel_goal_after_accept) { + action_client->async_cancel_goal(goal_handle); + } + } }; send_goal_options.feedback_callback = [this]( @@ -211,9 +221,10 @@ class ActionClientManager : public rclcpp::Node bool enable_action_server_introspection_; bool enable_action_client_introspection_; - void execute(const std::shared_ptr goal_handle) + void execute( + const std::shared_ptr goal_handle, + std::chrono::seconds exec_goal_time) { - rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); auto result = std::make_shared(); @@ -222,7 +233,11 @@ class ActionClientManager : public rclcpp::Node goal_handle->publish_feedback(feedback); } - loop_rate.sleep(); + 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); 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 61de50ed95..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 @@ -287,14 +287,8 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) } record_options.services = service_list; - // Covert action name to related topic name - auto action_list = node.declare_parameter>( + record_options.actions = node.declare_parameter>( "record.actions", std::vector()); - for (auto & action : action_list) { - auto one_action_topic_list = rosbag2_cpp::action_name_to_action_interface_names(action); - std::move(one_action_topic_list.begin(), one_action_topic_list.end(), - std::back_inserter(record_options.actions)); - } record_options.exclude_topics = node.declare_parameter>( "record.exclude_topics", std::vector()); @@ -310,14 +304,8 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) } record_options.exclude_service_events = exclude_service_list; - // Covert action name to related topic name - auto exclude_action_list = node.declare_parameter>( + record_options.exclude_actions = node.declare_parameter>( "record.exclude_actions", std::vector()); - for (auto & action : exclude_action_list) { - auto one_action_topic_list = rosbag2_cpp::action_name_to_action_interface_names(action); - std::move(one_action_topic_list.begin(), one_action_topic_list.end(), - std::back_inserter(record_options.exclude_actions)); - } record_options.rmw_serialization_format = node.declare_parameter("record.rmw_serialization_format", "cdr"); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 8af0b45a2c..4fe05685e1 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -250,3 +250,52 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_service_actio 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_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 3a3f94e4c0..ea3296be57 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -489,8 +489,8 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_action_recording) ASSERT_TRUE(action_manager_b2->send_goal()); // One action include at least 8 messages - // Goal request received, Goal response sent, 3 feedback, Result request received - // Result response sent, status + // 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]() { @@ -499,7 +499,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_action_recording) 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(Ge(expected_at_least_messages_size))); + 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 @@ -589,8 +589,8 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_actions_action_recording) ASSERT_TRUE(action_manager_b2->send_goal()); // One action include at least 8 messages - // Goal request received, Goal response sent, 3 feedback, Result request received - // Result response sent, status + // 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]() { @@ -599,7 +599,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_actions_action_recording) 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(Ge(expected_at_least_messages_size))); + 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 From 7a29fcce1bffd0ea0d216a67d7d1ebaa9527c3fb Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 27 Mar 2025 12:52:16 +0800 Subject: [PATCH 04/18] genstub Signed-off-by: Barry Xu --- rosbag2_py/rosbag2_py/_transport.pyi | 3 +++ 1 file changed, 3 insertions(+) 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] From cb175c282704ac63580b882a60e188c3cb6a8e02 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 27 Mar 2025 13:36:56 +0800 Subject: [PATCH 05/18] Optimize some parts of the code Signed-off-by: Barry Xu --- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 14 +++++-- rosbag2_py/CMakeLists.txt | 1 + .../src/rosbag2_py/format_action_info.cpp | 23 +---------- .../src/rosbag2_py/format_service_info.cpp | 25 +----------- rosbag2_py/src/rosbag2_py/format_utils.cpp | 40 +++++++++++++++++++ rosbag2_py/src/rosbag2_py/format_utils.hpp | 26 ++++++++++++ 6 files changed, 81 insertions(+), 48 deletions(-) create mode 100644 rosbag2_py/src/rosbag2_py/format_utils.cpp create mode 100644 rosbag2_py/src/rosbag2_py/format_utils.hpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index aee9d4dd09..20ed525c6b 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -28,10 +28,16 @@ const size_t kServiceEventTypePostfixLen = strlen(kServiceEventTopicTypePostfix) bool is_service_event_topic(const std::string & topic_name, const std::string & topic_type) { - if ((topic_name.length() <= kServiceEventTopicPostfixLen) || - (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != - RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) || - (topic_name.find("/_action/") != std::string::npos)) + if (topic_name.find("/_action/") != std::string::npos) { + return false; + } + + if (topic_name.length() <= kServiceEventTopicPostfixLen) { + return false; + } + + if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { return false; } diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index bd146d935c..15bdb9463a 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -94,6 +94,7 @@ pybind11_add_module(_info 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/src/rosbag2_py/format_action_info.cpp b/rosbag2_py/src/rosbag2_py/format_action_info.cpp index 44da48237d..748e4d48bb 100644 --- a/rosbag2_py/src/rosbag2_py/format_action_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_action_info.cpp @@ -16,29 +16,10 @@ #include #include "format_action_info.hpp" -#include "rosbag2_cpp/action_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++; - } +#include "format_utils.hpp" - 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]; -} +#include "rosbag2_cpp/action_utils.hpp" -} // namespace namespace rosbag2_py { diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index e340803dff..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 { 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_ From 0e23198a4caf9c34bc50d0dcfd8a6bab0a194ec9 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 28 Mar 2025 17:19:21 +0800 Subject: [PATCH 06/18] Fix an issue with the display when there is no service data Signed-off-by: Barry Xu --- rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index b316352af6..391a9237ef 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -409,6 +409,8 @@ 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: "; From a4f21903732f89d7f83f0ec00f54f288e10c3e7c Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 28 Mar 2025 18:08:56 +0800 Subject: [PATCH 07/18] Address review comments Signed-off-by: Barry Xu --- rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp | 2 +- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 2 +- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 2 +- rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp | 7 ++++--- rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp | 2 +- rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp | 2 +- 6 files changed, 9 insertions(+), 8 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp index bdf9f5681b..087e505803 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp @@ -50,7 +50,7 @@ action_interface_name_to_action_name(const std::string & topic_name); // Note that cancel_goal event topic and status topic return "" ROSBAG2_CPP_PUBLIC std::string -action_interface_type_to_action_type(const std::string & topic_type); +get_action_type(const std::string & topic_type); ROSBAG2_CPP_PUBLIC ActionInterfaceType diff --git a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp index c000070e0e..7929f5091a 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -82,7 +82,7 @@ bool is_topic_belong_to_action( return std::regex_search(topic_type, pattern); } -std::string action_interface_type_to_action_type(const std::string & topic_type) +std::string get_action_type(const std::string & topic_type) { std::string action_type; diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 8c69050a0d..d9251c88ff 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -195,7 +195,7 @@ Info::read_service_and_action_info( // Update action type. Note: cancel_goal event topic and status topic cannot get type if (action_info->type.empty()) { - action_info->type = action_interface_type_to_action_type(t.type); + action_info->type = get_action_type(t.type); } // Update action_interface_name_to_action_name_map to speed up following code. diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 657b9212e8..8ac63c9450 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -226,12 +226,13 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic std::string topic_type; if (is_topic_belong_to_action(topic_with_type.name, topic_with_type.type)) { - // The following two action topic types cannot retrieve the action type. + // The following two action types cannot be retrieved from the topic type. // - xxx/_action/cancel_goal/_service_event (action_msgs/srv/CancelGoal_Event) // - xxx/_action/status (action_msgs/msg/GoalStatusArray) - topic_type = action_interface_type_to_action_type(topic_with_type.type); + topic_type = get_action_type(topic_with_type.type); - // TODO(Barry.Xu): get_full_text() not support action type. Need to implement it. + // TODO(Barry.Xu): LocalMessageDefinitionSource::get_full_text(topic_type) doesn't support + // action type. Need to implement it. // Now action type return empty message definition. definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_type); } else { diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp index 8ece07c36e..77973e3526 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp @@ -99,7 +99,7 @@ TEST_F(ActionUtilsTest, check_action_topic_type_to_action_type) for (const auto & test_data : all_test_data) { EXPECT_EQ( - rosbag2_cpp::action_interface_type_to_action_type(test_data.first), + rosbag2_cpp::get_action_type(test_data.first), test_data.second ); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index 391a9237ef..bd1afc8003 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -213,7 +213,7 @@ void filter_service_and_action_info( // 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::action_interface_type_to_action_type(topic.topic_metadata.type); + rosbag2_cpp::get_action_type(topic.topic_metadata.type); } switch (action_interface_type) { From a56f29565afa8ab18a266de141327cef3fe0dc6b Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sat, 29 Mar 2025 20:43:38 +0800 Subject: [PATCH 08/18] Change a variable name to make the code clearer and more readable Signed-off-by: Barry Xu --- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index d9251c88ff..0d540e5e95 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -250,7 +250,7 @@ Info::read_service_and_action_info( if (action_interface_type != ActionInterfaceType::Unknown) { auto action_name = action_interface_name_to_action_name_map[bag_msg->topic_name]; - auto action_service_info = action_process_info[action_name]; + auto action = action_process_info[action_name]; // Handle action service event topic switch (msg.event_type) { @@ -258,15 +258,12 @@ Info::read_service_and_action_info( case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: { if (action_interface_type == ActionInterfaceType::SendGoalEvent) { - action_service_info-> - send_goal_service.request[msg.client_gid].emplace(msg.sequence_number); + action->send_goal_service.request[msg.client_gid].emplace(msg.sequence_number); } else if (action_interface_type == ActionInterfaceType::GetResultEvent) { - action_service_info-> - get_result_service.request[msg.client_gid].emplace(msg.sequence_number); + action->get_result_service.request[msg.client_gid].emplace(msg.sequence_number); } else { // TopicsInAction::CancelGoalEvent - action_service_info-> - cancel_goal_service.request[msg.client_gid].emplace(msg.sequence_number); + action->cancel_goal_service.request[msg.client_gid].emplace(msg.sequence_number); } break; } @@ -274,15 +271,12 @@ Info::read_service_and_action_info( case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: { if (action_interface_type == ActionInterfaceType::SendGoalEvent) { - action_service_info-> - send_goal_service.response[msg.client_gid].emplace(msg.sequence_number); + action->send_goal_service.response[msg.client_gid].emplace(msg.sequence_number); } else if (action_interface_type == ActionInterfaceType::GetResultEvent) { - action_service_info-> - get_result_service.response[msg.client_gid].emplace(msg.sequence_number); + action->get_result_service.response[msg.client_gid].emplace(msg.sequence_number); } else { // TopicsInAction::CancelGoalEvent - action_service_info-> - cancel_goal_service.response[msg.client_gid].emplace(msg.sequence_number); + action->cancel_goal_service.response[msg.client_gid].emplace(msg.sequence_number); } break; } From fb1286d50cd0cf3535ef3ed57a1935117531c4b4 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sat, 29 Mar 2025 21:48:30 +0800 Subject: [PATCH 09/18] Address review comments Signed-off-by: Barry Xu --- .../include/rosbag2_cpp/action_utils.hpp | 3 +- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 10 +-- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 61 ++++++++----------- .../rosbag2_cpp/writers/sequential_writer.cpp | 2 +- .../test/rosbag2_cpp/test_action_utils.cpp | 2 +- .../src/rosbag2_py/format_bag_metadata.cpp | 2 +- 6 files changed, 37 insertions(+), 43 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp index 087e505803..da31118048 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp @@ -46,11 +46,12 @@ ROSBAG2_CPP_PUBLIC std::string action_interface_name_to_action_name(const std::string & topic_name); +// Get the action type from action interface type for display info // Call this function after is_topic_belong_to_action() return true // Note that cancel_goal event topic and status topic return "" ROSBAG2_CPP_PUBLIC std::string -get_action_type(const std::string & topic_type); +get_action_type_for_info(const std::string & interface_type); ROSBAG2_CPP_PUBLIC ActionInterfaceType diff --git a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp index 7929f5091a..145d628a8c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -82,28 +82,28 @@ bool is_topic_belong_to_action( return std::regex_search(topic_type, pattern); } -std::string get_action_type(const std::string & topic_type) +std::string get_action_type_for_info(const std::string & interface_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)) { + if (std::regex_search(interface_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")); + interface_type.substr(0, interface_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")); + interface_type.substr(0, interface_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")); + interface_type.substr(0, interface_type.length() - std::strlen("_FeedbackMessage")); break; case ActionInterfaceType::CancelGoalEvent: case ActionInterfaceType::Status: diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 0d540e5e95..94b6b5c3f0 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -77,73 +77,66 @@ struct action_service_req_resp_info service_req_resp_info get_result_service; }; -inline void calculate_message_counts( - std::unordered_map & message_map, - size_t & count) +inline size_t calculate_number_of_messages( + std::unordered_map & message_map) { - count = 0; + size_t message_count = 0; for (auto & [client_id, message_list] : message_map) { - count += message_list.size(); + message_count += message_list.size(); } + + return message_count; } using action_analysis = std::unordered_map>; -inline void summary_action_service_info( +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) { - size_t count = 0; // Get the number of request from all clients for send_goal - calculate_message_counts(action_info->send_goal_service.request, count); - all_action_info[action_name]->send_goal_service_msg_count.first = count; + 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 - count = 0; - calculate_message_counts(action_info->cancel_goal_service.request, count); - all_action_info[action_name]->cancel_goal_service_msg_count.first = count; + 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 - count = 0; - calculate_message_counts(action_info->get_result_service.request, count); - all_action_info[action_name]->get_result_service_msg_count.first = count; + 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 - count = 0; - calculate_message_counts(action_info->send_goal_service.response, count); - all_action_info[action_name]->send_goal_service_msg_count.second = count; + 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 - count = 0; - calculate_message_counts(action_info->cancel_goal_service.response, count); - all_action_info[action_name]->cancel_goal_service_msg_count.second = count; + 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 - count = 0; - calculate_message_counts(action_info->get_result_service.response, count); - all_action_info[action_name]->get_result_service_msg_count.second = count; + 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 summary_service_info( +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) { - size_t count = 0; // Get the number of request from all clients - calculate_message_counts(service_info->request, count); - all_service_info[topic_name]->request_count = count; + all_service_info[topic_name]->request_count = + calculate_number_of_messages(service_info->request); - count = 0; // Get the number of response from all clients - calculate_message_counts(service_info->response, count); - all_service_info[topic_name]->response_count = count; + all_service_info[topic_name]->response_count = + calculate_number_of_messages(service_info->response); } } } // namespace @@ -195,7 +188,7 @@ Info::read_service_and_action_info( // 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(t.type); + action_info->type = get_action_type_for_info(t.type); } // Update action_interface_name_to_action_name_map to speed up following code. @@ -305,7 +298,7 @@ Info::read_service_and_action_info( } // Process action_process_info to get the number of request and response - summary_action_service_info(action_process_info, all_action_info); + update_action_service_info_with_num_req_resp(action_process_info, all_action_info); // Covert all_action_info to output_action_info for (auto & [action_name, action_info] : all_action_info) { @@ -313,7 +306,7 @@ Info::read_service_and_action_info( } // Process service_process_info to get the number of request and response - summary_service_info(service_process_info, all_service_info); + 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) { diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 8ac63c9450..41e9f95561 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -229,7 +229,7 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic // The following two action types cannot be retrieved from the topic type. // - xxx/_action/cancel_goal/_service_event (action_msgs/srv/CancelGoal_Event) // - xxx/_action/status (action_msgs/msg/GoalStatusArray) - topic_type = get_action_type(topic_with_type.type); + topic_type = get_action_type_for_info(topic_with_type.type); // TODO(Barry.Xu): LocalMessageDefinitionSource::get_full_text(topic_type) doesn't support // action type. Need to implement it. diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp index 77973e3526..f2af84766d 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp @@ -99,7 +99,7 @@ TEST_F(ActionUtilsTest, check_action_topic_type_to_action_type) for (const auto & test_data : all_test_data) { EXPECT_EQ( - rosbag2_cpp::get_action_type(test_data.first), + rosbag2_cpp::get_action_type_for_info(test_data.first), test_data.second ); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index bd1afc8003..bf2a57b661 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -213,7 +213,7 @@ void filter_service_and_action_info( // 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(topic.topic_metadata.type); + rosbag2_cpp::get_action_type_for_info(topic.topic_metadata.type); } switch (action_interface_type) { From e4fd9ef659f0b57c078ce59ed0806c38a9ca7134 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 00:32:08 -0700 Subject: [PATCH 10/18] Simplify if-else block with a shorter name service_info Signed-off-by: Michael Orlov --- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 94b6b5c3f0..78b277e6bb 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -279,16 +279,15 @@ Info::read_service_and_action_info( } } 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_process_info[bag_msg->topic_name]->request[msg.client_gid].emplace( - msg.sequence_number); + 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_process_info[bag_msg->topic_name]->response[msg.client_gid].emplace( - msg.sequence_number); + service_info->response[msg.client_gid].emplace(msg.sequence_number); break; default: throw std::range_error("Invalid service event type " + From 63723f57102176fd1640bf87d2f657c0a9ebf5cf Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 00:48:03 -0700 Subject: [PATCH 11/18] Throw exception by default in the get_action_type_for_info(topic_type) Signed-off-by: Michael Orlov --- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp index 145d628a8c..89e7ac175f 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -107,7 +107,9 @@ std::string get_action_type_for_info(const std::string & interface_type) 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; From 8fcfbdeab56b2b0ccb79ea0333a9232a492e7ad3 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 00:49:38 -0700 Subject: [PATCH 12/18] Remove type conversion in sequential_writer - Rationale: We shall store the original message definition for recorded topics by design. This is also true for service events. Signed-off-by: Michael Orlov --- .../rosbag2_cpp/writers/sequential_writer.cpp | 33 +++---------------- 1 file changed, 5 insertions(+), 28 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 41e9f95561..5a82824721 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -29,9 +29,6 @@ #include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/logging.hpp" -#include "rosbag2_cpp/action_utils.hpp" -#include "rosbag2_cpp/service_utils.hpp" - #include "rosbag2_storage/default_storage_id.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -223,31 +220,11 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic return; } rosbag2_storage::MessageDefinition definition; - - std::string topic_type; - if (is_topic_belong_to_action(topic_with_type.name, topic_with_type.type)) { - // The following two action types cannot be retrieved from the topic type. - // - xxx/_action/cancel_goal/_service_event (action_msgs/srv/CancelGoal_Event) - // - xxx/_action/status (action_msgs/msg/GoalStatusArray) - topic_type = get_action_type_for_info(topic_with_type.type); - - // TODO(Barry.Xu): LocalMessageDefinitionSource::get_full_text(topic_type) doesn't support - // action type. Need to implement it. - // Now action type return empty message definition. - definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_type); - } else { - 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); - } catch (DefinitionNotFoundError &) { - definition = rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_type); - } + try { + definition = message_definitions_.get_full_text(topic_with_type.type); + } catch (DefinitionNotFoundError &) { + definition = + rosbag2_storage::MessageDefinition::empty_message_definition_for(topic_with_type.type); } create_topic(topic_with_type, definition); From 99103f8cc7af01340ed45c269e0f1ba4b946e912 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 00:53:28 -0700 Subject: [PATCH 13/18] Calculate actions status and feedback statistics in read_service_and_action_info Signed-off-by: Michael Orlov --- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 8 ++++- rosbag2_py/src/rosbag2_py/_info.cpp | 53 +++------------------------- 2 files changed, 11 insertions(+), 50 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 78b277e6bb..f6b6d92f75 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -229,7 +229,13 @@ Info::read_service_and_action_info( if (action_interface_type == ActionInterfaceType::Feedback || action_interface_type == ActionInterfaceType::Status) { - continue; // Skip the feedback and status topic for action + 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( diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 5c13d7668e..7fe3f0b309 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -85,56 +85,11 @@ class Info std::vector> all_actions_info; for (auto & file_info : metadata_info.files) { - auto [output_service_info, output_action_info] = info_->read_service_and_action_info( - uri + "/" + file_info.path, - metadata_info.storage_identifier); - if (!output_service_info.empty()) { - all_services_info.insert( - all_services_info.end(), - output_service_info.begin(), - output_service_info.end()); - } - if (!output_action_info.empty()) { - all_actions_info.insert( - all_actions_info.end(), - output_action_info.begin(), - output_action_info.end()); - } - } + auto [service_info, action_info] = info_->read_service_and_action_info( + uri + "/" + file_info.path, metadata_info.storage_identifier); - // Fill in the number of feedback and status messages for the action info from - // metadata_info.topics_with_message_count - if (!all_actions_info.empty()) { - for (auto & [topic_metadata, message_count] : metadata_info.topics_with_message_count) { - auto action_interface_type = - rosbag2_cpp::get_action_interface_type(topic_metadata.name); - if (rosbag2_cpp::is_topic_belong_to_action(action_interface_type, topic_metadata.type)) { - switch (action_interface_type) { - case rosbag2_cpp::ActionInterfaceType::Feedback: - case rosbag2_cpp::ActionInterfaceType::Status: - { - auto action_info_iter = std::find_if(all_actions_info.begin(), - all_actions_info.end(), - [topic_name = topic_metadata.name](const auto & action_info){ - return action_info->name == - rosbag2_cpp::action_interface_name_to_action_name(topic_name); - }); - if (action_info_iter == all_actions_info.end()) { - break; - } - - if (action_interface_type == rosbag2_cpp::ActionInterfaceType::Feedback) { - (*action_info_iter)->feedback_topic_msg_count = message_count; - } else { - (*action_info_iter)->status_topic_msg_count = message_count; - } - break; - } - default: - break; - } - } - } + 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 = {}; From c85ed3ca1ac41192f346391f19d481d52897622c Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 00:54:24 -0700 Subject: [PATCH 14/18] Address some other small style changes and nitpicks Signed-off-by: Michael Orlov --- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 10 ++++----- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 20 ++++++++--------- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 22 +++++++++---------- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 4 ++-- .../src/rosbag2_py/format_action_info.cpp | 6 ++--- .../src/rosbag2_py/format_bag_metadata.cpp | 2 -- .../src/rosbag2_py/info_sorting_method.hpp | 2 +- .../rosbag2_transport/record_options.hpp | 8 +++---- 8 files changed, 34 insertions(+), 40 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index f72e3b30db..d0d4190234 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -59,11 +59,11 @@ class ROSBAG2_CPP_PUBLIC Info virtual rosbag2_storage::BagMetadata read_metadata( const std::string & uri, const std::string & storage_id = ""); - virtual - std::pair>, - std::vector>> - read_service_and_action_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 index 89e7ac175f..355d3f4fa9 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -53,6 +53,16 @@ bool is_topic_belong_to_action(const std::string & topic_name, const std::string 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; @@ -72,16 +82,6 @@ std::string action_interface_name_to_action_name(const std::string & topic_name) return action_name; } -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 get_action_type_for_info(const std::string & interface_type) { std::string action_type; diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index f6b6d92f75..902ecfce13 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -141,10 +141,11 @@ inline void update_service_info_with_num_req_resp( } } // namespace -std::pair>, - std::vector>> -Info::read_service_and_action_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}); @@ -192,11 +193,7 @@ Info::read_service_and_action_info( } // Update action_interface_name_to_action_name_map to speed up following code. - if (action_interface_name_to_action_name_map.find(t.name) == - action_interface_name_to_action_name_map.end()) - { - action_interface_name_to_action_name_map[t.name] = action_name; - } + 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); @@ -212,7 +209,7 @@ Info::read_service_and_action_info( 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(); @@ -240,8 +237,9 @@ Info::read_service_and_action_info( 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 + " !"); diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index 20ed525c6b..6727f2a2cb 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -28,11 +28,11 @@ const size_t kServiceEventTypePostfixLen = strlen(kServiceEventTopicTypePostfix) bool is_service_event_topic(const std::string & topic_name, const std::string & topic_type) { - if (topic_name.find("/_action/") != std::string::npos) { + if (topic_name.length() <= kServiceEventTopicPostfixLen) { return false; } - if (topic_name.length() <= kServiceEventTopicPostfixLen) { + if (topic_name.find("/_action/") != std::string::npos) { return false; } diff --git a/rosbag2_py/src/rosbag2_py/format_action_info.cpp b/rosbag2_py/src/rosbag2_py/format_action_info.cpp index 748e4d48bb..ba01ce1a24 100644 --- a/rosbag2_py/src/rosbag2_py/format_action_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_action_info.cpp @@ -31,9 +31,8 @@ format_action_info( const InfoSortingMethod sort_method) { std::stringstream info_stream; - const std::string action_info_string = "Action information: "; info_stream << "Actions: " << action_info_list.size() << std::endl; - info_stream << action_info_string; + info_stream << "Action information: "; if (action_info_list.empty()) { return info_stream.str(); @@ -82,8 +81,7 @@ format_action_info( std::vector sorted_idx = generate_sorted_idx(action_info_list, sort_method); - auto number_of_actions = action_info_list.size(); - for (size_t j = 0; j < number_of_actions; ++j) { + for (size_t j = 0; j < action_info_list.size(); ++j) { print_action_info(action_info_list[sorted_idx[j]]); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index bf2a57b661..3593e50f66 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -259,8 +259,6 @@ void filter_service_and_action_info( for (auto & action_info : action_info_map) { action_info_list.emplace_back(action_info.second); } - - return; } void format_service_with_type( diff --git a/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp b/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp index 2217b4f8f4..341264b9d5 100644 --- a/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp +++ b/rosbag2_py/src/rosbag2_py/info_sorting_method.hpp @@ -63,7 +63,7 @@ std::vector generate_sorted_idx( std::vector generate_sorted_idx( const std::vector> & actions, - const InfoSortingMethod sort_method); + const InfoSortingMethod sort_method = InfoSortingMethod::NAME); std::vector generate_sorted_idx( const std::vector> & services, diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index f31a55a45b..53d52de01e 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -36,12 +36,12 @@ struct RecordOptions bool is_discovery_disabled = false; std::vector topics; std::vector topic_types; - std::vector services; // service event topic - std::vector actions; // action name list + 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_actions; // action name list + 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 = ""; From 5c31b04cfcbeb9a3541fe3c79fce65379cf105c8 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 16:46:23 -0700 Subject: [PATCH 15/18] Add default section with exception to sorting functions Signed-off-by: Michael Orlov --- .../src/rosbag2_py/info_sorting_method.cpp | 70 ++++++++++--------- 1 file changed, 38 insertions(+), 32 deletions(-) diff --git a/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp b/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp index 02a5adfe2f..45c4d8e8a3 100644 --- a/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp +++ b/rosbag2_py/src/rosbag2_py/info_sorting_method.cpp @@ -144,19 +144,22 @@ std::vector generate_sorted_idx( 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; + 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; } @@ -182,26 +185,29 @@ std::vector generate_sorted_idx( 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; + 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; + 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; } From 8e69169dd1ba0193fc1df3502f4a9aaff0af4c60 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 16:46:54 -0700 Subject: [PATCH 16/18] Use structural bindings in the test_action_utils.cpp Signed-off-by: Michael Orlov --- .../test/rosbag2_cpp/test_action_utils.cpp | 23 ++++++++----------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp index f2af84766d..a5444a0270 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_action_utils.cpp @@ -60,10 +60,9 @@ TEST_F(ActionUtilsTest, check_is_topic_related_to_action) {{"/abc/_action/status", "action_msgs/msg/GoalStatus"}, false} }; - for (const auto & test_data : all_test_data) { - EXPECT_TRUE( - rosbag2_cpp::is_topic_belong_to_action( - std::get<0>(test_data.first), std::get<1>(test_data.first)) == test_data.second); + 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); } } @@ -80,9 +79,8 @@ TEST_F(ActionUtilsTest, check_action_topic_name_to_action_name) {"/abc/action/status", ""} }; - for (const auto & test_data : all_test_data) { - EXPECT_TRUE( - rosbag2_cpp::action_interface_name_to_action_name(test_data.first) == test_data.second); + for (const auto & [topic_name, action_name] : all_test_data) { + EXPECT_TRUE(rosbag2_cpp::action_interface_name_to_action_name(topic_name) == action_name); } } @@ -97,11 +95,8 @@ TEST_F(ActionUtilsTest, check_action_topic_type_to_action_type) {"action_msgs/msg/GoalStatusArray", ""} }; - for (const auto & test_data : all_test_data) { - EXPECT_EQ( - rosbag2_cpp::get_action_type_for_info(test_data.first), - test_data.second - ); + for (const auto & [topic_type, action_type] : all_test_data) { + EXPECT_EQ(rosbag2_cpp::get_action_type_for_info(topic_type), action_type); } } @@ -118,8 +113,8 @@ TEST_F(ActionUtilsTest, check_get_action_interface_type) {"/abc/action/status", rosbag2_cpp::ActionInterfaceType::Unknown} }; - for (const auto & test_data : all_test_data) { - EXPECT_EQ(rosbag2_cpp::get_action_interface_type(test_data.first), test_data.second); + for (const auto & [topic_name, action_interface_type] : all_test_data) { + EXPECT_EQ(rosbag2_cpp::get_action_interface_type(topic_name), action_interface_type); } } From 6bb2be1540966d304594b62768b2335a794620f3 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 29 Mar 2025 00:46:04 -0700 Subject: [PATCH 17/18] Add Doxygen comments to the action_utils.hpp Signed-off-by: Michael Orlov --- .../include/rosbag2_cpp/action_utils.hpp | 36 ++++++++++++++++--- rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp | 10 +++--- 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp index da31118048..950768197c 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/action_utils.hpp @@ -22,6 +22,7 @@ namespace rosbag2_cpp { +/// \brief Enum class for action introspection interface types. enum class ActionInterfaceType { SendGoalEvent, @@ -32,31 +33,56 @@ enum class ActionInterfaceType 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); -// Call this function after is_topic_belong_to_action() return true +/// \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); -// Get the action type from action interface type for display info -// Call this function after is_topic_belong_to_action() return true -// Note that cancel_goal event topic and status topic return "" +/// \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 & interface_type); +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); diff --git a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp index 355d3f4fa9..ece759a5cb 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/action_utils.cpp @@ -82,28 +82,28 @@ std::string action_interface_name_to_action_name(const std::string & topic_name) return action_name; } -std::string get_action_type_for_info(const std::string & interface_type) +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(interface_type, pattern)) { + if (std::regex_search(topic_type, pattern)) { switch (topic_type_enum) { case ActionInterfaceType::SendGoalEvent: // Remove the postfix "_SendGoal_Event" action_type = - interface_type.substr(0, interface_type.length() - std::strlen("_SendGoal_Event")); + topic_type.substr(0, topic_type.length() - std::strlen("_SendGoal_Event")); break; case ActionInterfaceType::GetResultEvent: // Remove the postfix "_GetResult_Event" action_type = - interface_type.substr(0, interface_type.length() - std::strlen("_GetResult_Event")); + topic_type.substr(0, topic_type.length() - std::strlen("_GetResult_Event")); break; case ActionInterfaceType::Feedback: // Remove the postfix "_FeedbackMessage" action_type = - interface_type.substr(0, interface_type.length() - std::strlen("_FeedbackMessage")); + topic_type.substr(0, topic_type.length() - std::strlen("_FeedbackMessage")); break; case ActionInterfaceType::CancelGoalEvent: case ActionInterfaceType::Status: From e196b67df7540dd8fab470919560158187cb9b32 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 31 Mar 2025 22:10:23 -0700 Subject: [PATCH 18/18] Avoid using ContainsRegex on Windows to handle the '|' character Signed-off-by: Barry Xu --- .../test_rosbag2_info_end_to_end.cpp | 75 ++++++++++--------- 1 file changed, 40 insertions(+), 35 deletions(-) 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 642f16ed1b..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 @@ -127,48 +128,52 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { "\nEnd: Mar 14 2025 .*:.*:.*\\..* \\(.*\\..*\\)" "\nMessages: .*" "\nTopic information: ")); - EXPECT_THAT( - output, ContainsRegex( - "Topic: /test_topic1 | Type: std_msgs/msg/String | Count: //d+ | " - "Size Contribution: //+d B | Serialization Format: cdr\n")); - EXPECT_THAT( - output, ContainsRegex( - "Topic: /test_topic2 | Type: std_msgs/msg/String | Count: //d+ | " - "Size Contribution: //+d 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")); - EXPECT_THAT( - output, ContainsRegex( - "Service: /test_service1 | Type: example_interfaces/srv/AddTwoInts | Request Count: \\d+ | " - "Response Count: \\d+ | Size Contribution: \\d+ B | Serialization Format: cdr\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)); - EXPECT_THAT( - output, ContainsRegex( - "Service: /test_service2 | Type: example_interfaces/srv/AddTwoInts | Request Count: \\d+ | " - "Response Count: \\d+ | Size Contribution: \\d+ B | Serialization Format: cdr\n")); + 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:")); - EXPECT_THAT( - output, ContainsRegex( - "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_THAT( - output, ContainsRegex( - "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+")); + 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) {