From 92b5beb778be45a519ff1d8f888f208a356d98b4 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 14 Mar 2025 20:50:17 +0800 Subject: [PATCH 01/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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) { From bdc6e521e63339aabada21985917c396f8143196 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sun, 30 Mar 2025 15:55:50 +0800 Subject: [PATCH 19/19] Implement rosbag2 action play Signed-off-by: Barry Xu --- README.md | 7 + ros2bag/ros2bag/verb/burst.py | 6 + ros2bag/ros2bag/verb/play.py | 28 +- rosbag2_cpp/CMakeLists.txt | 1 + rosbag2_py/rosbag2_py/_transport.pyi | 3 + rosbag2_py/src/rosbag2_py/_storage.cpp | 8 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 5 +- .../rosbag2_storage/storage_filter.hpp | 11 + rosbag2_storage_mcap/src/mcap_storage.cpp | 47 +- .../test_mcap_topic_filter.cpp | 364 +++++++- .../sqlite_storage.cpp | 76 +- .../test_sqlite_topic_filter.cpp | 373 +++++++- .../action_server_manager.hpp | 149 +++ rosbag2_transport/CMakeLists.txt | 4 + .../rosbag2_transport/play_options.hpp | 16 +- .../include/rosbag2_transport/player.hpp | 15 + .../player_action_client.hpp | 179 ++++ .../config_options_from_node_params.cpp | 13 +- .../src/rosbag2_transport/play_options.cpp | 2 + .../src/rosbag2_transport/player.cpp | 598 +++++++++--- .../player_action_client.cpp | 491 ++++++++++ .../test/resources/player_node_params.yaml | 3 + .../test/rosbag2_transport/mock_player.hpp | 24 +- .../mock_sequential_reader.hpp | 8 + .../rosbag2_play_test_fixture.hpp | 3 + .../test/rosbag2_transport/test_burst.cpp | 162 ++++ .../test_composable_player.cpp | 8 + .../test/rosbag2_transport/test_play.cpp | 867 +++++++++++++++++- .../test/rosbag2_transport/test_play_loop.cpp | 6 +- 29 files changed, 3243 insertions(+), 234 deletions(-) create mode 100644 rosbag2_test_common/include/rosbag2_test_common/action_server_manager.hpp create mode 100644 rosbag2_transport/include/rosbag2_transport/player_action_client.hpp create mode 100644 rosbag2_transport/src/rosbag2_transport/player_action_client.cpp diff --git a/README.md b/README.md index 7e73aa1bea..b19773e2ce 100644 --- a/README.md +++ b/README.md @@ -167,6 +167,8 @@ Options: Space-delimited list of topics to play. * `--services`: Space-delimited list of services to play. +* `--actions`: + Space-delimited list of actions to play. * `-e,--regex`: Play only topics and services matches with regular expression. * `-x,--exclude-regex`: @@ -175,6 +177,8 @@ Options: Space-delimited list of topics not to play. * `--exclude-services`: Space-delimited list of services not to play. +* `--exclude-actions`: + Space-delimited list of actions not to play. * `--message-order {received,sent}`: The reference to use for bag message chronological ordering. Choices: reception timestamp (`received`), publication timestamp (`sent`). @@ -282,12 +286,15 @@ output_bags: topic_types: [] all_services: false services: [] + all_actions: false + actions: [] rmw_serialization_format: "" # defaults to using the format of the input topic regex: "" exclude_regex: "" exclude_topics: [] exclude_topic_types: [] exclude_services: [] + exclude_actions: [] compression_mode: "" compression_format: "" compression_queue_size: 1 diff --git a/ros2bag/ros2bag/verb/burst.py b/ros2bag/ros2bag/verb/burst.py index c9cd96eb64..11638481a8 100644 --- a/ros2bag/ros2bag/verb/burst.py +++ b/ros2bag/ros2bag/verb/burst.py @@ -48,6 +48,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '--services', type=str, default=[], nargs='+', help='services to replay, separated by space. At least one service needs to be ' "specified. If this parameter isn\'t specified, all services will be replayed.") + parser.add_argument( + '--actions', type=str, default=[], nargs='+', + help='actions to replay, separated by space. At least one action needs to be ' + "specified. If this parameter isn\'t specified, all actions will be replayed.") parser.add_argument( '--qos-profile-overrides-path', type=FileType('r'), help='Path to a yaml file defining overrides of the QoS profile for specific topics.') @@ -97,6 +101,8 @@ def main(self, *, args): # noqa: D102 play_options.topics_to_filter = args.topics # Convert service name to service event topic name play_options.services_to_filter = convert_service_to_service_event_topic(args.services) + # Covert action name to action related topic names + play_options.actions_to_filter = args.actions play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = False play_options.topic_remapping_options = topic_remapping diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index d92ad04fd0..b99e778482 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -60,18 +60,24 @@ def add_arguments(self, parser, cli_name): # noqa: D102 parser.add_argument( '--services', type=str, default=[], metavar='service', nargs='+', help='Space-delimited list of services to play.') + parser.add_argument( + '--actions', type=str, default=[], metavar='action', nargs='+', + help='Space-delimited list of actions to play.') parser.add_argument( '-e', '--regex', default='', - help='Play only topics and services matches with regular expression.') + help='Play only topics, services and actions matches with regular expression.') parser.add_argument( '-x', '--exclude-regex', default='', - help='regular expressions to exclude topics and services from replay.') + help='regular expressions to exclude topics, services and actions from replay.') parser.add_argument( '--exclude-topics', type=str, default=[], metavar='topic', nargs='+', help='Space-delimited list of topics not to play.') parser.add_argument( '--exclude-services', type=str, default=[], metavar='service', nargs='+', help='Space-delimited list of services not to play.') + parser.add_argument( + '--exclude-actions', type=str, default=[], metavar='action', nargs='+', + help='Space-delimited list of actions not to play.') parser.add_argument( '--qos-profile-overrides-path', type=FileType('r'), help='Path to a yaml file defining overrides of the QoS profile for specific topics.') @@ -162,12 +168,20 @@ def add_arguments(self, parser, cli_name): # noqa: D102 parser.add_argument( '--publish-service-requests', action='store_true', default=False, help='Publish recorded service requests instead of recorded service events') + parser.add_argument( + '--send-actions-as-client', action='store_true', default=False, + help='Send the send_goal request, cancel_goal request, and get_result request ' + 'respectively based on the recorded send_goal, cancel_goal, and get_result ' + 'event messages. Note that the messages from action\'s "status topic" and ' + '"feedback topic" will not be sent because they are expected to be sent from ' + 'the action server side.') parser.add_argument( '--service-requests-source', default='service_introspection', choices=['service_introspection', 'client_introspection'], help='Determine the source of the service requests to be replayed. This option only ' - 'makes sense if the "--publish-service-requests" option is set. By default,' - ' the service requests replaying from recorded service introspection message.') + 'makes sense if the "--publish-service-requests" or "--send-actions-as-client" ' + 'option is set. By default, the service requests replaying from recorded ' + 'service introspection message.') parser.add_argument( '--message-order', default='received', choices=['received', 'sent'], @@ -271,6 +285,8 @@ def main(self, *, args): # noqa: D102 # Convert service name to service event topic name play_options.services_to_filter = convert_service_to_service_event_topic(args.services) + play_options.actions_to_filter = args.actions + play_options.regex_to_filter = args.regex play_options.exclude_regex_to_filter = args.exclude_regex @@ -278,6 +294,9 @@ def main(self, *, args): # noqa: D102 play_options.exclude_service_events_to_filter = \ convert_service_to_service_event_topic(args.exclude_services) + play_options.exclude_actions_to_filter = \ + args.exclude_actions if args.exclude_actions else [] + play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = args.loop play_options.topic_remapping_options = topic_remapping @@ -299,6 +318,7 @@ def main(self, *, args): # noqa: D102 play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message play_options.publish_service_requests = args.publish_service_requests + play_options.send_actions_as_client = args.send_actions_as_client if not args.service_requests_source or \ args.service_requests_source == 'service_introspection': play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index df76e5c2be..7d159942eb 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} PUBLIC pluginlib::pluginlib rclcpp::rclcpp + rclcpp_action::rclcpp_action rcpputils::rcpputils rcutils::rcutils rmw::rmw diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index d7dc16ba40..1a20d7bdc6 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -20,12 +20,14 @@ class MessageOrder: def value(self) -> int: ... class PlayOptions: + actions_to_filter: List[str] clock_publish_frequency: float clock_publish_on_topic_publish: bool clock_topics: List[str] delay: float disable_keyboard_controls: bool disable_loan_message: bool + exclude_actions_to_filter: List[str] exclude_regex_to_filter: str exclude_service_events_to_filter: List[str] exclude_topics_to_filter: List[str] @@ -40,6 +42,7 @@ class PlayOptions: rate: float read_ahead_queue_size: int regex_to_filter: str + send_actions_as_client: bool service_requests_source: Incomplete services_to_filter: List[str] start_offset: float diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index e12898053d..6d513f1422 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -128,19 +128,23 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageFilter") .def( pybind11::init< - std::vector, std::vector, std::string, - std::vector, std::vector, std::string>(), + std::vector, std::vector, std::vector, std::string, + std::vector, std::vector, std::vector, std::string>(), pybind11::arg("topics") = std::vector(), pybind11::arg("services_events") = std::vector(), + pybind11::arg("actions") = std::vector(), pybind11::arg("regex") = "", pybind11::arg("exclude_topics") = std::vector(), pybind11::arg("exclude_service_events") = std::vector(), + pybind11::arg("exclude_actions") = std::vector(), pybind11::arg("regex_to_exclude") = "") .def_readwrite("topics", &rosbag2_storage::StorageFilter::topics) .def_readwrite("services_events", &rosbag2_storage::StorageFilter::services_events) + .def_readwrite("actions", &rosbag2_storage::StorageFilter::actions_interfaces) .def_readwrite("regex", &rosbag2_storage::StorageFilter::regex) .def_readwrite("exclude_topics", &rosbag2_storage::StorageFilter::exclude_topics) .def_readwrite("exclude_service_events", &rosbag2_storage::StorageFilter::exclude_service_events) + .def_readwrite("exclude_actions", &rosbag2_storage::StorageFilter::exclude_actions_interfaces) .def_readwrite("regex_to_exclude", &rosbag2_storage::StorageFilter::regex_to_exclude); pybind11::class_(m, "MessageDefinition") diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 7d9cf09f8f..142080f4ed 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -525,10 +525,12 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("rate", &PlayOptions::rate) .def_readwrite("topics_to_filter", &PlayOptions::topics_to_filter) .def_readwrite("services_to_filter", &PlayOptions::services_to_filter) + .def_readwrite("actions_to_filter", &PlayOptions::actions_to_filter) .def_readwrite("regex_to_filter", &PlayOptions::regex_to_filter) .def_readwrite("exclude_regex_to_filter", &PlayOptions::exclude_regex_to_filter) .def_readwrite("exclude_topics_to_filter", &PlayOptions::exclude_topics_to_filter) .def_readwrite("exclude_service_events_to_filter", &PlayOptions::exclude_services_to_filter) + .def_readwrite("exclude_actions_to_filter", &PlayOptions::exclude_actions_to_filter) .def_property( "topic_qos_profile_overrides", &PlayOptions::getTopicQoSProfileOverrides, @@ -561,12 +563,13 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout) .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message) .def_readwrite("publish_service_requests", &PlayOptions::publish_service_requests) + .def_readwrite("send_actions_as_client", &PlayOptions::send_actions_as_client) .def_readwrite("service_requests_source", &PlayOptions::service_requests_source) .def_readwrite("message_order", &PlayOptions::message_order) ; py::enum_(m, "ServiceRequestsSource") - .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::SERVICE_INTROSPECTION) + .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::SERVER_INTROSPECTION) .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION) ; diff --git a/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp b/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp index c54de1b069..b3dcee4c5c 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp @@ -34,6 +34,12 @@ struct StorageFilter // are returned. std::vector services_events; + // Action interface names to whitelist when reading a bag. Only messages + // matching these specified action interface names will be returned. If + // list is empty, the filter is ignored and all messages of actions are + // returned. + std::vector actions_interfaces; + // Regular expression of topic names and service name to whitelist when // playing a bag.Only messages matching these specified topics or services // will be returned. If the string is empty, the filter is ignored and all @@ -51,6 +57,11 @@ struct StorageFilter // are returned. std::vector exclude_service_events = {}; + // Action interface names to blacklist when reading a bag. Only messages + // unmatching these service event topics will be returned. If list is empty, + // the filter is ignored and all messages of actions are returned. + std::vector exclude_actions_interfaces = {}; + // Regular expression of topic names and service events names to blacklist when // playing a bag. Only messages not matching these topics and service events will // be returned. If the string is empty, the filter is ignored and all messages diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index cbade144c4..5d61e8db1f 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -235,8 +235,6 @@ class ROSBAG2_STORAGE_MCAP_PUBLIC MCAPStorage rosbag2_storage::storage_interfaces::IOFlag io_flag, const std::string & storage_config_uri); - static bool is_topic_name_a_service_event(const std::string_view topic_name); - /// \brief Check if topic match with the selection criteria by the white list or regex during /// data read. /// \details There is assumption that by default all topics shall be selected if none of the @@ -566,7 +564,9 @@ bool MCAPStorage::read_and_enqueue_message() return true; } -bool MCAPStorage::is_topic_name_a_service_event(const std::string_view topic_name) +namespace +{ +bool is_topic_name_a_service_event(const std::string_view topic_name) { // The origin definition is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX static const char * service_event_topic_postfix = "/_service_event"; @@ -582,6 +582,24 @@ bool MCAPStorage::is_topic_name_a_service_event(const std::string_view topic_nam return true; } +// The postfix of the action internal topics and service event topics +const std::vector ActionInterfacePostfix = { + "/_action/send_goal/_service_event", "/_action/cancel_goal/_service_event", + "/_action/get_result/_service_event", "/_action/feedback", "/_action/status"}; + +bool is_topic_name_related_to_action(const std::string_view topic_name) +{ + for (const auto & postfix : ActionInterfacePostfix) { + if (topic_name.length() > postfix.length() && + topic_name.compare(topic_name.length() - postfix.length(), postfix.length(), postfix) == + 0) { + return true; + } + } + return false; +} +} // namespace + template bool MCAPStorage::is_topic_selected_by_white_list_or_regex(const std::string_view topic_name, const T & white_list, @@ -644,18 +662,25 @@ void MCAPStorage::reset_iterator() options.readOrder = read_order_; auto topic_filter = [this](std::string_view topic) { - bool topic_a_service_event = is_topic_name_a_service_event(topic); - - const auto & include_list = - topic_a_service_event ? storage_filter_.services_events : storage_filter_.topics; + std::vector * include_list = nullptr; + std::vector * exclude_list = nullptr; + + if (is_topic_name_related_to_action(topic)) { + include_list = &storage_filter_.actions_interfaces; + exclude_list = &storage_filter_.exclude_actions_interfaces; + } else if (is_topic_name_a_service_event(topic)) { + include_list = &storage_filter_.services_events; + exclude_list = &storage_filter_.exclude_service_events; + } else { + include_list = &storage_filter_.topics; + exclude_list = &storage_filter_.exclude_topics; + } - const auto & exclude_list = topic_a_service_event ? storage_filter_.exclude_service_events - : storage_filter_.exclude_topics; // if topic not found in exclude list or regex_to_exclude - if (!is_topic_in_black_list_or_exclude_regex(topic, exclude_list, + if (!is_topic_in_black_list_or_exclude_regex(topic, *exclude_list, storage_filter_.regex_to_exclude)) { // if topic selected by include list or regex - if (is_topic_selected_by_white_list_or_regex(topic, include_list, storage_filter_.regex)) { + if (is_topic_selected_by_white_list_or_regex(topic, *include_list, storage_filter_.regex)) { return true; } } diff --git a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp index 0974d7e015..b5184a8704 100644 --- a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp +++ b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp @@ -49,9 +49,18 @@ class McapTopicFilterTestFixture : public testing::Test fs::remove_all(tmp_dir_path); fs::create_directories(tmp_dir_path); - std::vector topics = {"topic1", "service_topic1/_service_event", "topic2", - "service_topic2/_service_event", "topic3"}; - + std::vector topics = {"topic1", + "service_topic1/_service_event", + "action1/_action/send_goal/_service_event", + "topic2", + "action_topic2/_action/cancel_goal/_service_event", + "service_topic2/_service_event", + "action1/_action/get_result/_service_event", + "topic3", + "action1/_action/feedback", + "action1/_action/status"}; + + // Since the topic type will not be used in testing, use a fixed value. rosbag2_storage::TopicMetadata topic_metadata_1 = { 1u, topics[0], "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}; rosbag2_storage::TopicMetadata topic_metadata_2 = { @@ -62,6 +71,16 @@ class McapTopicFilterTestFixture : public testing::Test 4u, topics[3], "std_msgs/msg/String", "cdr", {rclcpp::QoS(4)}, "type_hash4"}; rosbag2_storage::TopicMetadata topic_metadata_5 = { 5u, topics[4], "std_msgs/msg/String", "cdr", {rclcpp::QoS(5)}, "type_hash5"}; + rosbag2_storage::TopicMetadata topic_metadata_6 = { + 6u, topics[5], "std_msgs/msg/String", "cdr", {rclcpp::QoS(6)}, "type_hash6"}; + rosbag2_storage::TopicMetadata topic_metadata_7 = { + 7u, topics[6], "std_msgs/msg/String", "cdr", {rclcpp::QoS(7)}, "type_hash7"}; + rosbag2_storage::TopicMetadata topic_metadata_8 = { + 8u, topics[7], "std_msgs/msg/String", "cdr", {rclcpp::QoS(8)}, "type_hash8"}; + rosbag2_storage::TopicMetadata topic_metadata_9 = { + 9u, topics[8], "std_msgs/msg/String", "cdr", {rclcpp::QoS(9)}, "type_hash9"}; + rosbag2_storage::TopicMetadata topic_metadata_10 = { + 10u, topics[9], "std_msgs/msg/String", "cdr", {rclcpp::QoS(10)}, "type_hash10"}; const rosbag2_storage::MessageDefinition definition = {"std_msgs/msg/String", "ros2msg", "string data", ""}; @@ -71,9 +90,14 @@ class McapTopicFilterTestFixture : public testing::Test string_messages = { std::make_tuple("topic1 message", 1, topic_metadata_1, definition), std::make_tuple("service event topic 1 message", 2, topic_metadata_2, definition), - std::make_tuple("topic2 message", 3, topic_metadata_3, definition), - std::make_tuple("service event topic 2 message", 4, topic_metadata_4, definition), - std::make_tuple("topic3 message", 5, topic_metadata_5, definition)}; + std::make_tuple("action1 send goal event message", 3, topic_metadata_3, definition), + std::make_tuple("topic2 message", 3, topic_metadata_4, definition), + std::make_tuple("action_topic2 cancel goal event message", 4, topic_metadata_5, definition), + std::make_tuple("service event topic 2 message", 4, topic_metadata_6, definition), + std::make_tuple("action1 get result event message", 5, topic_metadata_7, definition), + std::make_tuple("topic3 message", 5, topic_metadata_8, definition), + std::make_tuple("action1 feedback message", 6, topic_metadata_9, definition), + std::make_tuple("action1 status message", 7, topic_metadata_10, definition)}; rosbag2_storage::StorageFactory factory; rosbag2_storage::StorageOptions options; @@ -139,18 +163,43 @@ TEST_F(McapTopicFilterTestFixture, CanSelectTopicsAndServicesWithEmptyFilters) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("topic2")); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto fourth_message = readable_storage->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(fourth_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto fifth_message = readable_storage->read_next(); - EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + EXPECT_THAT(fifth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("action1/_action/get_result/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto ninth_message = readable_storage->read_next(); + EXPECT_THAT(ninth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto tenth_message = readable_storage->read_next(); + EXPECT_THAT(tenth_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -165,12 +214,35 @@ TEST_F(McapTopicFilterTestFixture, CanSelectWithTopicsListOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/get_result/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -185,16 +257,79 @@ TEST_F(McapTopicFilterTestFixture, CanSelectWithServiceEventsListOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/get_result/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto ninth_message = readable_storage->read_next(); + EXPECT_THAT(ninth_message->topic_name, Eq("action1/_action/status")); + + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectWithActionsListOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + // When an action is added to the filter, it translates to all related action topics being added. + storage_filter.actions_interfaces = {"action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; + readable_storage->set_filter(storage_filter); + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(third_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto fourth_message = readable_storage->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -209,19 +344,41 @@ TEST_F(McapTopicFilterTestFixture, TestResetFilter) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); - EXPECT_FALSE(readable_storage->has_next()); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/get_result/_service_event")); readable_storage->reset_filter(); EXPECT_TRUE(readable_storage->has_next()); - auto fourth_message = readable_storage->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto ninth_message = readable_storage->read_next(); + EXPECT_THAT(ninth_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -270,28 +427,68 @@ TEST_F(McapTopicFilterTestFixture, CanSelectFromServicesListAndRegexWithTopics) EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(McapTopicFilterTestFixture, CanSelectFromTopicsListAndServiceList) +TEST_F(McapTopicFilterTestFixture, CanSelectFromActionsListAndRegexWithTopics) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set service list and regex for topic + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.actions_interfaces = {"action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; + storage_filter.regex = "topic(1|3)"; // Add topic1 and topic3 + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectFromTopicsListServiceListAndActionList) { auto readable_storage = open_test_bag_for_read_only(); // Set topic list and service list rosbag2_storage::StorageFilter storage_filter{}; storage_filter.topics = {"topic2", "topic3"}; storage_filter.services_events = {"service_topic1/_service_event"}; + storage_filter.actions_interfaces = {"action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_THAT(third_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexOnly) +TEST_F(McapTopicFilterTestFixture, FilterTopicsServicesAndActionsWithRegexOnly) { auto readable_storage = open_test_bag_for_read_only(); // No topic list and service list. Only regex @@ -302,13 +499,19 @@ TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentTopicsList) +TEST_F(McapTopicFilterTestFixture, FilterTopicsServicesActionsWithRegexAndNonexistentTopicsList) { auto readable_storage = open_test_bag_for_read_only(); // Topics list with non-existent topic and regex with topic and service event @@ -320,13 +523,19 @@ TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexisten EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentServicesList) +TEST_F(McapTopicFilterTestFixture, FilterTopicsServicesActionsWithRegexAndNonexistentServicesList) { auto readable_storage = open_test_bag_for_read_only(); // Service events list with non-existent service and regex with topic and service event @@ -338,33 +547,77 @@ TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexisten EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithBlackListsAndExcludeRegexOnly) +TEST_F(McapTopicFilterTestFixture, FilterTopicsServicesActionsWithRegexAndNonexistentActionsList) { auto readable_storage = open_test_bag_for_read_only(); - // No topic list, service list and regex. - // Set excluded topic list, excluded service list and excluded regex + // Action list with non-existent action and regex with action + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.actions_interfaces = {"non-existent_action/_action/send_goal/_service_event", + "non-existent_action/_action/cancel_goal/_service_event", + "non-existent_action/_action/get_result/_service_event", + "non-existent_action/_action/feedback", + "non-existent_action/_action/status"}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterTopicsServicesAndActionsWithBlackListsAndExcludeRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // No topic list, service list, action list and regex. + // Set excluded topic list, excluded service list, excluded action list and excluded regex rosbag2_storage::StorageFilter storage_filter{}; storage_filter.exclude_topics = {"topic1"}; storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + storage_filter.exclude_actions_interfaces = { + "action1/_action/send_goal/_service_event", "action1/_action/cancel_goal/_service_event", + "action1/_action/get_result/_service_event", "action1/_action/feedback", + "action1/_action/status"}; storage_filter.regex_to_exclude = "^topic3$"; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesExcludeOverlapsWithIncludeLists) +TEST_F(McapTopicFilterTestFixture, FilterTopicsServicesAndActionsExcludeOverlapsWithIncludeLists) { auto readable_storage = open_test_bag_for_read_only(); // Exclude from include lists @@ -372,19 +625,40 @@ TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesExcludeOverlapsWithInc storage_filter.topics = {"topic1", "topic2", "topic3"}; storage_filter.services_events = {"service_topic1/_service_event", "service_topic2/_service_event"}; + storage_filter.actions_interfaces = {"action1/_action/send_goal/_service_event", + "action1/_action/cancel_goal/_service_event", + "action1/_action/get_result/_service_event", + "action1/_action/feedback", + "action1/_action/status", + "action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; storage_filter.exclude_topics = {"topic1"}; storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + storage_filter.exclude_actions_interfaces = { + "action1/_action/send_goal/_service_event", "action1/_action/cancel_goal/_service_event", + "action1/_action/get_result/_service_event", "action1/_action/feedback", + "action1/_action/status"}; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_THAT(third_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -393,13 +667,18 @@ TEST_F(McapTopicFilterTestFixture, FilterWithRegexAndExcludeRegex) auto readable_storage = open_test_bag_for_read_only(); // Set regex and excluded regex. rosbag2_storage::StorageFilter storage_filter{}; - storage_filter.regex = ".*topic1.*"; - storage_filter.regex_to_exclude = ".*service.*"; + storage_filter.regex = ".*topic2.*"; + storage_filter.regex_to_exclude = "service.*"; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); - EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -414,11 +693,30 @@ TEST_F(McapTopicFilterTestFixture, FilterWithExcludeRegexOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action1/_action/get_result/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto fifth_message = readable_storage->read_next(); EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } diff --git a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp index 20dc2ef49a..957e66adf1 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -572,6 +572,19 @@ void prepare_included_topics_filter( } } + // Add topic name related to actions + if (!storage_filter.actions_interfaces.empty()) { + if (!included_topic_names_str.empty()) { + included_topic_names_str += ","; + } + for (auto & action_topic : storage_filter.actions_interfaces) { + included_topic_names_str += "'" + action_topic + "'"; + if (&action_topic != &storage_filter.actions_interfaces.back()) { + included_topic_names_str += ","; + } + } + } + std::string topics_filter_str; if (!included_topic_names_str.empty()) { topics_filter_str.append("(topics.name IN (" + included_topic_names_str + "))"); @@ -584,28 +597,56 @@ void prepare_included_topics_filter( regex_filter_str = "(topics.name REGEXP '" + storage_filter.regex + "')"; } - static const char * regex_for_all_service_events_str = "(topics.name REGEXP '.*/_service_event')"; + static const char * regex_for_all_service_events_str = + "((topics.name REGEXP '.*/_service_event') and not (topics.name REGEXP '.*/_action/.*'))"; + static const char * regex_for_all_action_topics_str = "(topics.name REGEXP '.*/_action/.*')"; if (!topics_filter_str.empty() && !regex_filter_str.empty()) { // Note: Inclusive filter conditions shall be joined with OR - // Note: Even if services_events list or topics list is empty we shall not include regex for - // all service events or for all topics, because storage_filter.regex is not empty and shall - // dominate in this case. + // Note: Even if services_events list or topics list or actions list is empty we shall not + // include regex for all service events or for all topics or for all actions, because + // storage_filter.regex is not empty and shall dominate in this case. where_conditions.push_back("(" + topics_filter_str + " OR " + regex_filter_str + ")"); } else if (!topics_filter_str.empty()) { // Note: regex_filter_str is empty in this case if (!storage_filter.services_events.empty()) { - if (!storage_filter.topics.empty()) { + if (!storage_filter.topics.empty() && !storage_filter.actions_interfaces.empty()) { where_conditions.push_back(topics_filter_str); - } else { // if topics list empty and service_events not empty we shall include all topics + } else if (!storage_filter.actions_interfaces.empty()) { + // if topic list empty, service_events list not empty and actions list not empty, we shall + // include all topics + where_conditions.push_back( + "(" + topics_filter_str + " OR ( NOT " + regex_for_all_service_events_str + + " And NOT " + regex_for_all_action_topics_str + " ))"); + } else { + // If actions list empty, topics list empty and service_events list not empty, we shall + // include all actions where_conditions.push_back( "(" + topics_filter_str + " OR NOT " + regex_for_all_service_events_str + ")"); } - } else if (!storage_filter.topics.empty()) { - where_conditions.push_back( - "(" + topics_filter_str + " OR " + regex_for_all_service_events_str + ")"); - } else { - // This shall never happen unless someone will make incorrect changes in logic - throw std::logic_error("Either service_events list or topics list shall be not empty!"); + } else if (!storage_filter.actions_interfaces.empty()) { // service_events list empty + if (!storage_filter.topics.empty()) { + // If service_events list empty, topics list not empty and actions list not empty, we shall + // include all service events + where_conditions.push_back( + "(" + topics_filter_str + " OR " + regex_for_all_service_events_str + ")"); + } else { + // If service_events list empty, topics list empty and actions list not empty, we shall + // include all topics and services + where_conditions.push_back( + "(" + topics_filter_str + " OR NOT " + regex_for_all_action_topics_str + ")"); + } + } else { // services_event list empty and actions list empty + if (!storage_filter.topics.empty()) { + // If service_events list empty, action list empty and topics list not empty, we shall + // include all service_events and all actions + where_conditions.push_back( + "(" + topics_filter_str + " OR " + regex_for_all_service_events_str + " OR " + + regex_for_all_action_topics_str + ")"); + } else { + // This shall never happen unless someone will make incorrect changes in logic + throw std::logic_error("At least one of the following lists shall not be empty: " + "service_events list, topics list, or actions list."); + } } } else if (!regex_filter_str.empty()) { where_conditions.push_back(regex_filter_str); @@ -638,6 +679,17 @@ void prepare_excluded_topics_filter( } } + // Add topic name related to actions + if (!storage_filter.exclude_actions_interfaces.empty()) { + excluded_topic_names_str += ","; + for (auto & action_topic : storage_filter.exclude_actions_interfaces) { + excluded_topic_names_str += "'" + action_topic + "'"; + if (&action_topic != &storage_filter.exclude_actions_interfaces.back()) { + excluded_topic_names_str += ","; + } + } + } + if (!excluded_topic_names_str.empty()) { where_conditions.push_back("(topics.name NOT IN (" + excluded_topic_names_str + "))"); } diff --git a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp index 5428495226..ccb9bb7e4c 100644 --- a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp +++ b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp @@ -49,8 +49,16 @@ class SQLiteTopicFilterTestFixture : public testing::Test fs::remove_all(tmp_dir_path); fs::create_directories(tmp_dir_path); - std::vector topics = {"topic1", "service_topic1/_service_event", "topic2", - "service_topic2/_service_event", "topic3"}; + std::vector topics = {"topic1", + "service_topic1/_service_event", + "action1/_action/send_goal/_service_event", + "topic2", + "action_topic2/_action/cancel_goal/_service_event", + "service_topic2/_service_event", + "action1/_action/get_result/_service_event", + "topic3", + "action1/_action/feedback", + "action1/_action/status"}; rosbag2_storage::TopicMetadata topic_metadata_1 = { 1u, topics[0], "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}; @@ -62,6 +70,16 @@ class SQLiteTopicFilterTestFixture : public testing::Test 4u, topics[3], "std_msgs/msg/String", "cdr", {rclcpp::QoS(4)}, "type_hash4"}; rosbag2_storage::TopicMetadata topic_metadata_5 = { 5u, topics[4], "std_msgs/msg/String", "cdr", {rclcpp::QoS(5)}, "type_hash5"}; + rosbag2_storage::TopicMetadata topic_metadata_6 = { + 6u, topics[5], "std_msgs/msg/String", "cdr", {rclcpp::QoS(6)}, "type_hash6"}; + rosbag2_storage::TopicMetadata topic_metadata_7 = { + 7u, topics[6], "std_msgs/msg/String", "cdr", {rclcpp::QoS(7)}, "type_hash7"}; + rosbag2_storage::TopicMetadata topic_metadata_8 = { + 8u, topics[7], "std_msgs/msg/String", "cdr", {rclcpp::QoS(8)}, "type_hash8"}; + rosbag2_storage::TopicMetadata topic_metadata_9 = { + 9u, topics[8], "std_msgs/msg/String", "cdr", {rclcpp::QoS(9)}, "type_hash9"}; + rosbag2_storage::TopicMetadata topic_metadata_10 = { + 10u, topics[9], "std_msgs/msg/String", "cdr", {rclcpp::QoS(10)}, "type_hash10"}; const rosbag2_storage::MessageDefinition definition = {"std_msgs/msg/String", "ros2msg", "string data", ""}; @@ -71,9 +89,15 @@ class SQLiteTopicFilterTestFixture : public testing::Test string_messages = { std::make_tuple("topic1 message", 1, topic_metadata_1, definition), std::make_tuple("service event topic 1 message", 2, topic_metadata_2, definition), - std::make_tuple("topic2 message", 3, topic_metadata_3, definition), - std::make_tuple("service event topic 2 message", 4, topic_metadata_4, definition), - std::make_tuple("topic3 message", 5, topic_metadata_5, definition)}; + std::make_tuple("action1 send goal event message", 3, topic_metadata_3, definition), + std::make_tuple("topic2 message", 3, topic_metadata_4, definition), + std::make_tuple("action_topic2 cancel goal event message", 4, topic_metadata_5, definition), + std::make_tuple("service event topic 2 message", 4, topic_metadata_6, definition), + std::make_tuple("action1 get result event message", 5, topic_metadata_7, definition), + std::make_tuple("topic3 message", 5, topic_metadata_8, definition), + std::make_tuple("action1 feedback message", 6, topic_metadata_9, definition), + std::make_tuple("action1 status message", 7, topic_metadata_10, definition), + }; rosbag2_storage::StorageFactory factory; rosbag2_storage::StorageOptions options; @@ -139,18 +163,43 @@ TEST_F(SQLiteTopicFilterTestFixture, CanSelectTopicsAndServicesWithEmptyFilters) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("topic2")); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto fourth_message = readable_storage->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(fourth_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto fifth_message = readable_storage->read_next(); - EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + EXPECT_THAT(fifth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("action1/_action/get_result/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto ninth_message = readable_storage->read_next(); + EXPECT_THAT(ninth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto tenth_message = readable_storage->read_next(); + EXPECT_THAT(tenth_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -165,12 +214,35 @@ TEST_F(SQLiteTopicFilterTestFixture, CanSelectWithTopicsListOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/get_result/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -185,16 +257,80 @@ TEST_F(SQLiteTopicFilterTestFixture, CanSelectWithServiceEventsListOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/get_result/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto ninth_message = readable_storage->read_next(); + EXPECT_THAT(ninth_message->topic_name, Eq("action1/_action/status")); + + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectWithActionsListOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + // When an action is added to the filter, it translates to all related action topics being added. + storage_filter.actions_interfaces = { + "action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(third_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto fourth_message = readable_storage->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -209,19 +345,41 @@ TEST_F(SQLiteTopicFilterTestFixture, TestResetFilter) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); - EXPECT_FALSE(readable_storage->has_next()); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/get_result/_service_event")); readable_storage->reset_filter(); EXPECT_TRUE(readable_storage->has_next()); - auto fourth_message = readable_storage->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto eighth_message = readable_storage->read_next(); + EXPECT_THAT(eighth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto ninth_message = readable_storage->read_next(); + EXPECT_THAT(ninth_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -270,28 +428,70 @@ TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromServicesListAndRegexWithTopics EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromTopicsListAndServiceList) +TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromActionsListAndRegexWithTopics) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set service list and regex for topic + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.actions_interfaces = { + "action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; + storage_filter.regex = "topic(1|3)"; // Add topic1 and topic3 + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromTopicsListServiceListAndActionList) { auto readable_storage = open_test_bag_for_read_only(); // Set topic list and service list rosbag2_storage::StorageFilter storage_filter{}; storage_filter.topics = {"topic2", "topic3"}; storage_filter.services_events = {"service_topic1/_service_event"}; + storage_filter.actions_interfaces = { + "action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_THAT(third_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexOnly) +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsServicesAndActionsWithRegexOnly) { auto readable_storage = open_test_bag_for_read_only(); // No topic list and service list. Only regex @@ -302,13 +502,19 @@ TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentTopicsList) +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsServicesActionsWithRegexAndNonexistentTopicsList) { auto readable_storage = open_test_bag_for_read_only(); // Topics list with non-existent topic and regex with topic and service event @@ -320,13 +526,20 @@ TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexist EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentServicesList) +TEST_F(SQLiteTopicFilterTestFixture, + FilterTopicsServicesActionsWithRegexAndNonexistentServicesList) { auto readable_storage = open_test_bag_for_read_only(); // Service events list with non-existent service and regex with topic and service event @@ -338,33 +551,81 @@ TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexist EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithBlackListsAndExcludeRegexOnly) +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsServicesActionsWithRegexAndNonexistentActionsList) { auto readable_storage = open_test_bag_for_read_only(); - // No topic list, service list and regex. - // Set excluded topic list, excluded service list and excluded regex + // Action list with non-existent action and regex with action + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.actions_interfaces = { + "non-existent_action/_action/send_goal/_service_event", + "non-existent_action/_action/cancel_goal/_service_event", + "non-existent_action/_action/get_result/_service_event", + "non-existent_action/_action/feedback", + "non-existent_action/_action/status"}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, + FilterTopicsServicesAndActionsWithBlackListsAndExcludeRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // No topic list, service list, action list and regex. + // Set excluded topic list, excluded service list, excluded action list and excluded regex rosbag2_storage::StorageFilter storage_filter{}; storage_filter.exclude_topics = {"topic1"}; storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + storage_filter.exclude_actions_interfaces = { + "action1/_action/send_goal/_service_event", + "action1/_action/cancel_goal/_service_event", + "action1/_action/get_result/_service_event", + "action1/_action/feedback", + "action1/_action/status"}; storage_filter.regex_to_exclude = "^topic3$"; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } -TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesExcludeOverlapsWithIncludeLists) +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsServicesAndActionsExcludeOverlapsWithIncludeLists) { auto readable_storage = open_test_bag_for_read_only(); // Exclude from include lists @@ -372,19 +633,43 @@ TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesExcludeOverlapsWithI storage_filter.topics = {"topic1", "topic2", "topic3"}; storage_filter.services_events = {"service_topic1/_service_event", "service_topic2/_service_event"}; + storage_filter.actions_interfaces = { + "action1/_action/send_goal/_service_event", + "action1/_action/cancel_goal/_service_event", + "action1/_action/get_result/_service_event", + "action1/_action/feedback", + "action1/_action/status", + "action_topic2/_action/send_goal/_service_event", + "action_topic2/_action/cancel_goal/_service_event", + "action_topic2/_action/get_result/_service_event", + "action_topic2/_action/feedback", + "action_topic2/_action/status"}; storage_filter.exclude_topics = {"topic1"}; storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + storage_filter.exclude_actions_interfaces = { + "action1/_action/send_goal/_service_event", + "action1/_action/cancel_goal/_service_event", + "action1/_action/get_result/_service_event", + "action1/_action/feedback", + "action1/_action/status"}; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); auto third_message = readable_storage->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_THAT(third_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -393,13 +678,18 @@ TEST_F(SQLiteTopicFilterTestFixture, FilterWithRegexAndExcludeRegex) auto readable_storage = open_test_bag_for_read_only(); // Set regex and excluded regex. rosbag2_storage::StorageFilter storage_filter{}; - storage_filter.regex = ".*topic1.*"; - storage_filter.regex_to_exclude = ".*service.*"; + storage_filter.regex = ".*topic2.*"; + storage_filter.regex_to_exclude = "service.*"; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); - EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("action_topic2/_action/cancel_goal/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); } @@ -414,11 +704,30 @@ TEST_F(SQLiteTopicFilterTestFixture, FilterWithExcludeRegexOnly) EXPECT_TRUE(readable_storage->has_next()); auto first_message = readable_storage->read_next(); EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); auto second_message = readable_storage->read_next(); EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("action1/_action/send_goal/_service_event")); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("action1/_action/get_result/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); auto fifth_message = readable_storage->read_next(); EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + + EXPECT_TRUE(readable_storage->has_next()); + auto sixth_message = readable_storage->read_next(); + EXPECT_THAT(sixth_message->topic_name, Eq("action1/_action/feedback")); + + EXPECT_TRUE(readable_storage->has_next()); + auto seventh_message = readable_storage->read_next(); + EXPECT_THAT(seventh_message->topic_name, Eq("action1/_action/status")); + EXPECT_FALSE(readable_storage->has_next()); } diff --git a/rosbag2_test_common/include/rosbag2_test_common/action_server_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/action_server_manager.hpp new file mode 100644 index 0000000000..6bfa6bb5bb --- /dev/null +++ b/rosbag2_test_common/include/rosbag2_test_common/action_server_manager.hpp @@ -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. + +#ifndef ROSBAG2_TEST_COMMON__ACTION_SERVER_MANAGER_HPP_ +#define ROSBAG2_TEST_COMMON__ACTION_SERVER_MANAGER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/executors.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace rosbag2_test_common +{ +class ActionServerManager +{ +public: + ActionServerManager() + : action_server_node_(std::make_shared( + "action_server_manager_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions() + .start_parameter_event_publisher(false) + .enable_rosout(false) + .start_parameter_services(false))), + check_action_server_ready_node_(std::make_shared( + "check_service_ready_node_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions() + .start_parameter_event_publisher(false) + .enable_rosout(false) + .start_parameter_services(false))) + { + } + + ~ActionServerManager() + { + exec_.cancel(); + if (thread_.joinable()) { + thread_.join(); + } + } + + template + void setup_action( + std::string action_name, + size_t & request_count, + size_t & cancel_count, std::chrono::milliseconds exec_goal_time = 200ms) + { + auto handle_goal = [&request_count]( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + (void) uuid; + (void) goal; + ++request_count; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handle_cancel = [&cancel_count]( + const std::shared_ptr> goal_handle) + { + (void)goal_handle; + ++cancel_count; + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this, exec_goal_time]( + const std::shared_ptr> goal_handle) + { + (void)goal_handle; + auto execute_in_thread = + [this, goal_handle, exec_goal_time]() + { + return this->action_execute(goal_handle, exec_goal_time); + }; + std::thread{execute_in_thread}.detach(); + }; + + auto server = rclcpp_action::create_server( + action_server_node_, action_name, handle_goal, handle_cancel, handle_accepted); + action_servers_.emplace(action_name, server); + + auto client = rclcpp_action::create_client( + check_action_server_ready_node_, action_name); + action_clients_.emplace_back(client); + } + + void run_action_servers() + { + exec_.add_node(action_server_node_); + thread_ = std::thread( + [this]() { + exec_.spin(); + }); + } + + bool all_action_servers_ready() + { + for (auto client : action_clients_) { + if (!client->wait_for_action_server(std::chrono::seconds(2))) { + return false; + } + } + return true; + } + +private: + std::shared_ptr action_server_node_; + std::shared_ptr check_action_server_ready_node_; + std::unordered_map action_servers_; + std::vector action_clients_; + std::thread thread_; + rclcpp::executors::SingleThreadedExecutor exec_; + + template + void action_execute( + std::shared_ptr> goal_handle, + std::chrono::milliseconds exec_goal_time) + { + auto result = std::make_shared(); + std::this_thread::sleep_for(exec_goal_time); + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + return; + } + goal_handle->succeed(result); + } +}; + +} // namespace rosbag2_test_common + +#endif // ROSBAG2_TEST_COMMON__ACTION_SERVER_MANAGER_HPP_ diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 5f0e723e44..9313db5d08 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(keyboard_handler REQUIRED) find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) @@ -48,6 +49,7 @@ add_library(${PROJECT_NAME} SHARED src/rosbag2_transport/play_options.cpp src/rosbag2_transport/player_progress_bar.cpp src/rosbag2_transport/player_service_client.cpp + src/rosbag2_transport/player_action_client.cpp src/rosbag2_transport/reader_writer_factory.cpp src/rosbag2_transport/recorder.cpp src/rosbag2_transport/record_options.cpp @@ -59,6 +61,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC keyboard_handler::keyboard_handler rclcpp::rclcpp + rclcpp_action::rclcpp_action rosbag2_cpp::rosbag2_cpp ${rosbag2_interfaces_TARGETS} rosbag2_storage::rosbag2_storage @@ -105,6 +108,7 @@ ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( keyboard_handler rclcpp + rclcpp_action rosbag2_cpp rosbag2_interfaces rosbag2_storage diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index d925ba0400..f45d45cb2c 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -30,7 +30,7 @@ namespace rosbag2_transport { enum class ServiceRequestsSource : int8_t { - SERVICE_INTROSPECTION = 0, + SERVER_INTROSPECTION = 0, CLIENT_INTROSPECTION = 1 }; @@ -59,6 +59,11 @@ struct PlayOptions // If list is empty, the filter is ignored and all messages of services are played. std::vector services_to_filter = {}; + // Action names to whitelist when playing a bag. + // Only messages matching these specified actions will be played. If list is empty, + // the filter is ignored and all messages of actions are played. + std::vector actions_to_filter = {}; + // Regular expression of topic names and service name to whitelist when playing a bag. // Only messages matching these specified topics and services will be played. // If list is empty, the filter is ignored and all messages are played. @@ -72,6 +77,10 @@ struct PlayOptions // Only messages not matching these specified services will be played. std::vector exclude_services_to_filter = {}; + // List of action names to exclude when playing a bag. + // Only messages not matching these specified actions will be played. + std::vector exclude_actions_to_filter = {}; + // Regular expression of topic names and service name to exclude when playing a bag. // Only messages not matching these specified topics and services will be played. std::string exclude_regex_to_filter = ""; @@ -129,8 +138,11 @@ struct PlayOptions // Publish service requests instead of service events bool publish_service_requests = false; + // Publish the send_goal request, cancel_goal request, and get_result request + bool send_actions_as_client = false; + // The source of the service request - ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVER_INTROSPECTION; // The reference to use for bag message chronological ordering. // If messages are significantly disordered (within a single bag or across multiple bags), diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index 41c8c1b564..5f2a7ad3e6 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -31,6 +31,8 @@ #include "rclcpp/publisher.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp_action/generic_client.hpp" + #include "rosbag2_cpp/clocks/player_clock.hpp" #include "rosbag2_interfaces/msg/read_split_event.hpp" #include "rosbag2_interfaces/srv/get_rate.hpp" @@ -339,6 +341,19 @@ class Player : public rclcpp::Node ROSBAG2_TRANSPORT_PUBLIC std::unordered_map> get_service_clients(); + /// \brief Getter for clients corresponding to each action name + /// \return Hashtable representing action name to client + ROSBAG2_TRANSPORT_PUBLIC + std::unordered_map> + get_action_clients(); + + /// \brief Goal handle for action client is in processing. + /// \return true if goal handle is in processing, otherwise false. + /// \exception std::exception No action client is created for this action name. + ROSBAG2_TRANSPORT_PUBLIC + bool + goal_handle_in_process(std::string action_name, const rclcpp_action::GoalUUID & goal_id); + /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher ROSBAG2_TRANSPORT_PUBLIC diff --git a/rosbag2_transport/include/rosbag2_transport/player_action_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_action_client.hpp new file mode 100644 index 0000000000..a94a72d968 --- /dev/null +++ b/rosbag2_transport/include/rosbag2_transport/player_action_client.hpp @@ -0,0 +1,179 @@ +// 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_TRANSPORT__PLAYER_ACTION_CLIENT_HPP_ +#define ROSBAG2_TRANSPORT__PLAYER_ACTION_CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/types.h" +#include "rclcpp_action/generic_client.hpp" +#include +#include "rclcpp/rclcpp.hpp" + +#include "rosbag2_cpp/action_utils.hpp" + +namespace rosbag2_transport +{ + +class PlayerActionClient final +{ +public: + using GoalID = rclcpp_action::GoalUUID; + using IntrospectionMessageMembersPtr = + const rosidl_typesupport_introspection_cpp::MessageMembers *; + + enum class ServiceInterfaceInAction + { + SEND_GOAL_SERVICE, + CANCEL_GOAL_SERVICE, + GET_RESULT_SERVICE + }; + + enum class ServiceEventType + { + REQUEST_SENT = service_msgs::msg::ServiceEventInfo::REQUEST_SENT, + REQUEST_RECEIVED = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED, + RESPONSE_SENT = service_msgs::msg::ServiceEventInfo::RESPONSE_SENT, + RESPONSE_RECEIVED = service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED + }; + + explicit + PlayerActionClient( + rclcpp_action::GenericClient::SharedPtr generic_client, + std::string & action_name, + const std::string & action_type, + rclcpp::Logger logger); + + ~PlayerActionClient(); + + const std::string & get_action_name(); + + /// \brief Deserialize message to the type erased send goal service event + /// \param message - Serialized message + /// \return Shared pointer to the byte array with deserialized service event if success, + /// otherwise nullptr + std::shared_ptr deserialize_send_goal_service_event( + const rcl_serialized_message_t & message); + + /// \brief Deserialize message to the type erased cancel goal service event + /// \param message - Serialized message + /// \return Shared pointer to the byte array with deserialized service event if success, + /// otherwise nullptr + std::shared_ptr deserialize_cancel_goal_service_event( + const rcl_serialized_message_t & message); + + /// \brief Deserialize message to the type erased get result service event + /// \param message - Serialized message + /// \return Shared pointer to the byte array with deserialized service event if success, + /// otherwise nullptr + std::shared_ptr deserialize_get_result_service_event( + const rcl_serialized_message_t & message); + + /// \brief Extract the request data from the send_goal event message and asynchronously + // send the extracted request data. + void async_send_goal_request( + const std::shared_ptr & type_erased_send_goal_service_event); + + /// \brief Extract the request data from the cancel_goal event message and asynchronously + // send the extracted request data. + void async_send_cancel_request( + const std::shared_ptr & type_erased_cancel_goal_service_event); + + /// \brief Extract the request data from the get_result event message and asynchronously + // send the extracted request data. + void async_send_result_request( + const std::shared_ptr & type_erased_get_result_service_event); + + /// Wait until sent service requests will receive responses from service servers. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + bool wait_for_sent_requests_to_finish( + std::chrono::duration timeout = std::chrono::seconds(5)); + + ServiceEventType get_service_event_type( + const std::shared_ptr & type_erased_service_event, + ServiceInterfaceInAction service_type_in_action); + + rclcpp_action::GenericClient::SharedPtr generic_client() + { + return client_; + } + + bool gold_handle_in_processing(const GoalID & goal_id); + +private: + rclcpp_action::GenericClient::SharedPtr client_; + std::string action_name_; + std::string action_type_; + const rclcpp::Logger logger_; + + // Warning output once exceeding this value + const size_t maximum_goal_handle_size_ = 100; + + // Note: The action_ts_lib_ shall be a member variable to make sure that library loaded + // during the liveliness of the instance of this class, since we have raw pointers to its members. + std::shared_ptr action_ts_lib_; + + const rosidl_message_type_support_t * goal_service_event_type_ts_; + IntrospectionMessageMembersPtr goal_service_request_type_members_; + IntrospectionMessageMembersPtr goal_service_event_type_members_; + + const rosidl_message_type_support_t * cancel_service_event_type_ts_; + IntrospectionMessageMembersPtr cancel_service_request_type_members_; + IntrospectionMessageMembersPtr cancel_service_event_type_members_; + + const rosidl_message_type_support_t * result_service_event_type_ts_; + IntrospectionMessageMembersPtr result_service_request_type_members_; + IntrospectionMessageMembersPtr result_service_event_type_members_; + + rcutils_allocator_t allocator_ = rcutils_get_default_allocator(); + + std::unordered_map< + GoalID, rclcpp_action::GenericClientGoalHandle::SharedPtr, std::hash> + goal_id_to_goal_handle_map_; + std::mutex goal_id_to_goal_handle_map_mutex_; + + std::unordered_set> goal_ids_to_postpone_send_cancel_; + std::mutex goal_ids_to_postpone_send_cancel_mutex_; + + std::unordered_set> goal_ids_to_postpone_send_result_; + std::mutex goal_ids_to_postpone_send_result_mutex_; + + std::shared_ptr + deserialize_service_event( + const rosidl_message_type_support_t * service_event_type_support, + IntrospectionMessageMembersPtr service_event_members, + const rcl_serialized_message_t & message); + + bool is_request_service_event( + const std::shared_ptr & type_erased_service_event, + IntrospectionMessageMembersPtr service_event_members); + + // Must be called after check_if_service_event_type_is_request + bool get_goal_id_from_cancel_goal_service_event( + const std::shared_ptr & type_erased_cancel_goal_service_event, GoalID & goal_id); + + bool get_goal_id_from_get_result_service_event( + const std::shared_ptr & type_erased_get_result_service_event, GoalID & goal_id); +}; +} // namespace rosbag2_transport + +#endif // ROSBAG2_TRANSPORT__PLAYER_ACTION_CLIENT_HPP_ 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 04e4da693d..867cec56e4 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 @@ -141,6 +141,9 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) } play_options.services_to_filter = service_list; + play_options.actions_to_filter = node.declare_parameter>( + "play.actions_to_filter", std::vector()); + play_options.regex_to_filter = node.declare_parameter("play.regex_to_filter", ""); @@ -158,6 +161,9 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) } play_options.exclude_services_to_filter = exclude_service_list; + play_options.exclude_actions_to_filter = node.declare_parameter>( + "play.exclude_actions_to_filter", std::vector()); + std::string qos_profile_overrides_path = node.declare_parameter("play.qos_profile_overrides_path", ""); @@ -223,11 +229,11 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) auto service_requests_source = node.declare_parameter("play.service_requests_source", "SERVICE_INTROSPECTION"); if (service_requests_source == "SERVICE_INTROSPECTION") { - play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + play_options.service_requests_source = ServiceRequestsSource::SERVER_INTROSPECTION; } else if (service_requests_source == "CLIENT_INTROSPECTION") { play_options.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; } else { - play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + play_options.service_requests_source = ServiceRequestsSource::SERVER_INTROSPECTION; RCLCPP_ERROR( node.get_logger(), "play.service_requests_source doesn't support %s. It must be one of SERVICE_INTROSPECTION" @@ -238,6 +244,9 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) play_options.publish_service_requests = node.declare_parameter("play.publish_service_request", false); + play_options.send_actions_as_client = + node.declare_parameter("play.send_actions_as_client", false); + auto message_order = node.declare_parameter("play.message_order", "RECEIVED_TIMESTAMP"); if (message_order == "RECEIVED_TIMESTAMP") { diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index 72aefb57df..ccdc767354 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -32,10 +32,12 @@ Node convert::encode( node["rate"] = play_options.rate; node["topics_to_filter"] = play_options.topics_to_filter; node["services_to_filter"] = play_options.services_to_filter; + node["actions_to_filter"] = play_options.actions_to_filter; node["regex_to_filter"] = play_options.regex_to_filter; node["exclude_regex_to_filter"] = play_options.exclude_regex_to_filter; node["exclude_topics"] = play_options.exclude_topics_to_filter; node["exclude_services"] = play_options.exclude_services_to_filter; + node["exclude_actions"] = play_options.exclude_actions_to_filter; node["topic_qos_profile_overrides"] = YAML::convert>::encode( play_options.topic_qos_profile_overrides); diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index d45fb0ce4e..01fcf6b9cc 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -28,9 +28,11 @@ #include "rcl/graph.h" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/create_generic_client.hpp" #include "rcpputils/unique_lock.hpp" #include "rcutils/time.h" +#include "rosbag2_cpp/action_utils.hpp" #include "rosbag2_cpp/clocks/time_controller_clock.hpp" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/service_utils.hpp" @@ -38,6 +40,7 @@ #include "rosbag2_storage/storage_filter.hpp" #include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/config_options_from_node_params.hpp" +#include "rosbag2_transport/player_action_client.hpp" #include "rosbag2_transport/player_service_client.hpp" #include "rosbag2_transport/player_progress_bar.hpp" #include "rosbag2_transport/reader_writer_factory.hpp" @@ -194,6 +197,16 @@ class PlayerImpl /// \return Hashtable representing service name to client map std::unordered_map> get_service_clients(); + /// \brief Getter for clients corresponding to actions + /// \return Hashtable representing action name to client map + std::unordered_map> + get_action_clients(); + + /// \brief Goal handle for action client is in processing. + /// \return true if goal handle is in processing, otherwise false. + bool + goal_handle_in_process(std::string action_name, const rclcpp_action::GoalUUID & goal_id); + /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher rclcpp::Publisher::SharedPtr get_clock_publisher(); @@ -284,8 +297,10 @@ class PlayerImpl rclcpp::Publisher::SharedPtr clock_publisher_; using PlayerPublisherSharedPtr = std::shared_ptr; using PlayerServiceClientSharedPtr = std::shared_ptr; + using PlayerActionClientSharedPtr = std::shared_ptr; std::unordered_map publishers_; std::unordered_map service_clients_; + std::unordered_map action_clients_; private: rosbag2_storage::SerializedBagMessageSharedPtr take_next_message_from_queue(); @@ -298,6 +313,16 @@ class PlayerImpl void play_messages_from_queue(); void prepare_publishers(); bool publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message); + bool publish_message_by_player_publisher( + PlayerPublisherSharedPtr & player_publisher, + rosbag2_storage::SerializedBagMessageSharedPtr & message); + bool publish_message_by_player_service_client( + PlayerServiceClientSharedPtr & service_client, + rosbag2_storage::SerializedBagMessageSharedPtr & message); + bool publish_message_by_play_action_client( + PlayerActionClientSharedPtr & action_client, + rosbag2_cpp::ActionInterfaceType action_interface_type, + rosbag2_storage::SerializedBagMessageSharedPtr & message); static callback_handle_t get_new_on_play_msg_callback_handle(); void add_key_callback( KeyboardHandler::KeyCode key, @@ -455,6 +480,18 @@ PlayerImpl::PlayerImpl( owner_->get_namespace(), false); } + for (auto & action_topic : play_options_.actions_to_filter) { + action_topic = rclcpp::expand_topic_or_service_name( + action_topic, owner_->get_name(), + owner_->get_namespace(), false); + } + + for (auto & exclude_action_topic : play_options_.exclude_actions_to_filter) { + exclude_action_topic = rclcpp::expand_topic_or_service_name( + exclude_action_topic, owner_->get_name(), + owner_->get_namespace(), false); + } + { std::lock_guard lk(reader_mutex_); starting_time_ = std::numeric_limits::max(); @@ -951,6 +988,27 @@ std::unordered_map> +PlayerImpl::get_action_clients() +{ + std::unordered_map> + topic_to_action_client_map; + for (const auto & [action_name, action_client] : action_clients_) { + topic_to_action_client_map[action_name] = action_client->generic_client(); + } + return topic_to_action_client_map; +} + +bool +PlayerImpl::goal_handle_in_process(std::string action_name, const rclcpp_action::GoalUUID & goal_id) +{ + if (action_name.empty() || action_clients_.find(action_name) == action_clients_.end()) { + throw(std::invalid_argument("Action client for action '" + action_name + "' does not exist.")); + } + + return action_clients_[action_name]->gold_handle_in_processing(goal_id); +} + rclcpp::Publisher::SharedPtr PlayerImpl::get_clock_publisher() { return clock_publisher_; @@ -1169,8 +1227,15 @@ rcutils_time_point_value_t PlayerImpl::get_message_order_timestamp( namespace { +enum class TopicKind +{ + TOPIC, + SERVICE_EVENT_TOPIC, + ACTION_INTERFACE_TOPIC +}; + bool allow_topic( - bool is_service, + TopicKind topic_type, const std::string & topic_name, const rosbag2_storage::StorageFilter & storage_filter) { @@ -1178,25 +1243,46 @@ bool allow_topic( auto & exclude_topics = storage_filter.exclude_topics; auto & include_services = storage_filter.services_events; auto & exclude_services = storage_filter.exclude_service_events; + auto & include_actions = storage_filter.actions_interfaces; + auto & exclude_actions = storage_filter.exclude_actions_interfaces; auto & regex = storage_filter.regex; auto & regex_to_exclude = storage_filter.regex_to_exclude; - if (is_service) { - if (!exclude_services.empty()) { - auto it = std::find(exclude_services.begin(), exclude_services.end(), topic_name); - if (it != exclude_services.end()) { - return false; + // Check if topic is in the exclude list + switch (topic_type) { + case TopicKind::TOPIC: + { + if (!exclude_topics.empty()) { + auto it = std::find(exclude_topics.begin(), exclude_topics.end(), topic_name); + if (it != exclude_topics.end()) { + return false; + } + } + break; } - } - } else { - if (!exclude_topics.empty()) { - auto it = std::find(exclude_topics.begin(), exclude_topics.end(), topic_name); - if (it != exclude_topics.end()) { - return false; + case TopicKind::SERVICE_EVENT_TOPIC: + { + if (!exclude_services.empty()) { + auto it = std::find(exclude_services.begin(), exclude_services.end(), topic_name); + if (it != exclude_services.end()) { + return false; + } + } + break; + } + case TopicKind::ACTION_INTERFACE_TOPIC: + { + if (!exclude_topics.empty()) { + auto it = std::find(exclude_actions.begin(), exclude_actions.end(), topic_name); + if (it != exclude_actions.end()) { + return false; + } + } + break; } - } } + // Check if topic match the regex to exclude if (!regex_to_exclude.empty()) { std::smatch m; std::regex re(regex_to_exclude); @@ -1206,30 +1292,67 @@ bool allow_topic( } } - bool set_include = is_service ? !include_services.empty() : !include_topics.empty(); + bool set_include = false; + switch (topic_type) { + case TopicKind::TOPIC: + { + set_include = !include_topics.empty(); + break; + } + case TopicKind::SERVICE_EVENT_TOPIC: + { + set_include = !include_services.empty(); + break; + } + case TopicKind::ACTION_INTERFACE_TOPIC: + { + set_include = !include_actions.empty(); + break; + } + } bool set_regex = !regex.empty(); if (set_include || set_regex) { - if (is_service) { - auto iter = std::find(include_services.begin(), include_services.end(), topic_name); - if (iter == include_services.end()) { - // If include_service is set and regex isn't set, service must be in include_service. - if (!set_regex) { - return false; + switch (topic_type) { + case TopicKind::TOPIC: + { + auto iter = std::find(include_topics.begin(), include_topics.end(), topic_name); + if (iter == include_topics.end()) { + // If include_topics is set and regex isn't set, topic must be in include_topics. + if (!set_regex) { + return false; + } + } else { + return true; + } + break; } - } else { - return true; - } - } else { - auto iter = std::find(include_topics.begin(), include_topics.end(), topic_name); - if (iter == include_topics.end()) { - // If include_service is set and regex isn't set, service must be in include_service. - if (!set_regex) { - return false; + case TopicKind::SERVICE_EVENT_TOPIC: + { + auto iter = std::find(include_services.begin(), include_services.end(), topic_name); + if (iter == include_services.end()) { + // If include_service is set and regex isn't set, topic must be in include_service. + if (!set_regex) { + return false; + } + } else { + return true; + } + break; + } + case TopicKind::ACTION_INTERFACE_TOPIC: + { + auto iter = std::find(include_actions.begin(), include_actions.end(), topic_name); + if (iter == include_actions.end()) { + // If include_actions is set and regex isn't set, topic must be in include_actions. + if (!set_regex) { + return false; + } + } else { + return true; + } + break; } - } else { - return true; - } } if (set_regex) { @@ -1251,10 +1374,28 @@ void PlayerImpl::prepare_publishers() rosbag2_storage::StorageFilter storage_filter; storage_filter.topics = play_options_.topics_to_filter; storage_filter.services_events = play_options_.services_to_filter; + if (!play_options_.actions_to_filter.empty()) { + for (const auto & action : play_options_.actions_to_filter) { + auto action_interfaces = rosbag2_cpp::action_name_to_action_interface_names(action); + storage_filter.actions_interfaces.insert( + storage_filter.actions_interfaces.end(), + std::make_move_iterator(action_interfaces.begin()), + std::make_move_iterator(action_interfaces.end())); + } + } storage_filter.regex = play_options_.regex_to_filter; storage_filter.regex_to_exclude = play_options_.exclude_regex_to_filter; storage_filter.exclude_topics = play_options_.exclude_topics_to_filter; storage_filter.exclude_service_events = play_options_.exclude_services_to_filter; + if (!play_options_.exclude_actions_to_filter.empty()) { + for (const auto & action : play_options_.exclude_actions_to_filter) { + auto action_interfaces = rosbag2_cpp::action_name_to_action_interface_names(action); + storage_filter.exclude_actions_interfaces.insert( + storage_filter.exclude_actions_interfaces.end(), + std::make_move_iterator(action_interfaces.begin()), + std::make_move_iterator(action_interfaces.end())); + } + } for (const auto & [reader, _] : readers_with_options_) { reader->set_filter(storage_filter); } @@ -1302,15 +1443,55 @@ void PlayerImpl::prepare_publishers() } std::string topic_without_support_acked; for (const auto & topic : topics) { - const bool is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic.name, topic.type); - if (play_options_.publish_service_requests && is_service_event_topic) { + TopicKind topic_kind; + if (rosbag2_cpp::is_topic_belong_to_action(topic.name, topic.type)) { + topic_kind = TopicKind::ACTION_INTERFACE_TOPIC; + } else if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) { + topic_kind = TopicKind::SERVICE_EVENT_TOPIC; + } else { + topic_kind = TopicKind::TOPIC; + } + + if (topic_kind == TopicKind::ACTION_INTERFACE_TOPIC && play_options_.send_actions_as_client) { + // Check if action client was created + auto action_name = rosbag2_cpp::action_interface_name_to_action_name(topic.name); + if (action_clients_.find(action_name) != action_clients_.end()) { + continue; + } + + // filter action topics to add clients if necessary + if (!allow_topic(topic_kind, topic.name, storage_filter)) { + continue; + } + + auto action_type = rosbag2_cpp::get_action_type_for_info(topic.type); + if (action_type.empty()) { + // Skip cancel_goal event topics and status topics since these topic types cannot be + // converted to action type. + continue; + } + + try { + auto generic_client = rclcpp_action::create_generic_client( + owner_, action_name, action_type); + auto player_client = std::make_shared( + std::move(generic_client), action_name, action_type, owner_->get_logger()); + action_clients_.insert(std::make_pair(action_name, player_client)); + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + owner_->get_logger(), + "Ignoring a action '%s', reason: %s.", action_name.c_str(), e.what()); + } + } else if (topic_kind == TopicKind::SERVICE_EVENT_TOPIC && // NOLINT: conflict with uncrustify + play_options_.publish_service_requests) + { // Check if sender was created if (service_clients_.find(topic.name) != service_clients_.end()) { continue; } // filter service event topic to add client if necessary - if (!allow_topic(true, topic.name, storage_filter)) { + if (!allow_topic(topic_kind, topic.name, storage_filter)) { continue; } @@ -1334,7 +1515,7 @@ void PlayerImpl::prepare_publishers() } // filter topics to add publishers if necessary - if (!allow_topic(is_service_event_topic, topic.name, storage_filter)) { + if (!allow_topic(topic_kind, topic.name, storage_filter)) { continue; } @@ -1414,31 +1595,223 @@ void PlayerImpl::run_play_msg_post_callbacks( } } -bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) +bool PlayerImpl::publish_message_by_player_publisher( + PlayerPublisherSharedPtr & player_publisher, + rosbag2_storage::SerializedBagMessageSharedPtr & message) { - auto pub_iter = publishers_.find(message->topic_name); - if (pub_iter != publishers_.end()) { - bool message_published = false; - bool pre_callbacks_failed = true; + bool message_published = false; + bool pre_callbacks_failed = true; + try { + // Calling on play message pre-callbacks + run_play_msg_pre_callbacks(message); + pre_callbacks_failed = false; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(owner_->get_logger(), + "Failed to call on play message pre-callback on '" << message->topic_name << + "' topic. \nError: " << e.what()); + } + + if (!pre_callbacks_failed) { + try { + player_publisher->publish(rclcpp::SerializedMessage(*message->serialized_data)); + message_published = true; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(owner_->get_logger(), + "Failed to publish message on '" << message->topic_name << + "' topic. \nError: " << e.what()); + } + } + + try { + // Calling on play message post-callbacks + run_play_msg_post_callbacks(message); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(owner_->get_logger(), + "Failed to call on play message post-callback on '" << message->topic_name << + "' topic. \nError: " << e.what()); + } + return message_published; +} + +bool PlayerImpl::publish_message_by_player_service_client( + PlayerServiceClientSharedPtr & service_client, + rosbag2_storage::SerializedBagMessageSharedPtr & message) +{ + auto service_event = service_client->deserialize_service_event(*message->serialized_data); + if (!service_event) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to deserialize service event message for '" << + service_client->get_service_name() << "' service!\n"); + return false; + } + + try { + auto [service_event_type, client_gid] = + service_client->get_service_event_type_and_client_gid(service_event); + // Ignore response message + if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || + service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) + { + return false; + } + + if (play_options_.service_requests_source == ServiceRequestsSource::SERVER_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + return false; + } + + if (play_options_.service_requests_source == ServiceRequestsSource::CLIENT_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) + { + return false; + } + + if (!service_client->generic_client()->service_is_ready()) { + RCLCPP_ERROR( + owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", + service_client->get_service_name().c_str()); + return false; + } + + if (!service_client->is_service_event_include_request_message(service_event)) { + if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' was metadata only on service side!", + service_client->get_service_name().c_str()); + } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' client [ID: %s]` was metadata only!", + service_client->get_service_name().c_str(), + rosbag2_cpp::client_id_to_string(client_gid).c_str()); + } + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to send request on '" << + rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << + "' service. \nError: " << e.what()); + return false; + } + + // Calling on play message pre-callbacks + run_play_msg_pre_callbacks(message); + + bool message_published = false; + try { + service_client->async_send_request(service_event); + message_published = true; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to send request on '" << + rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << + "' service. \nError: " << e.what()); + } + + // Calling on play message post-callbacks + run_play_msg_post_callbacks(message); + return message_published; +} + +bool PlayerImpl::publish_message_by_play_action_client( + PlayerActionClientSharedPtr & action_client, + rosbag2_cpp::ActionInterfaceType action_interface_type, + rosbag2_storage::SerializedBagMessageSharedPtr & message) +{ + std::shared_ptr type_erased_service_event; + PlayerActionClient::ServiceEventType service_event_type; + switch (action_interface_type) { + case rosbag2_cpp::ActionInterfaceType::SendGoalEvent: + type_erased_service_event = + action_client->deserialize_send_goal_service_event(*message->serialized_data); + service_event_type = action_client->get_service_event_type( + type_erased_service_event, + PlayerActionClient::ServiceInterfaceInAction::SEND_GOAL_SERVICE); + break; + case rosbag2_cpp::ActionInterfaceType::CancelGoalEvent: + type_erased_service_event = + action_client->deserialize_cancel_goal_service_event(*message->serialized_data); + service_event_type = action_client->get_service_event_type( + type_erased_service_event, + PlayerActionClient::ServiceInterfaceInAction::CANCEL_GOAL_SERVICE); + break; + case rosbag2_cpp::ActionInterfaceType::GetResultEvent: + type_erased_service_event = + action_client->deserialize_get_result_service_event(*message->serialized_data); + service_event_type = action_client->get_service_event_type( + type_erased_service_event, + PlayerActionClient::ServiceInterfaceInAction::GET_RESULT_SERVICE); + break; + default: + // Never go here + break; + } + + if (!type_erased_service_event) { + return false; + } + + // Ignore all response messages + if (service_event_type == PlayerActionClient::ServiceEventType::RESPONSE_SENT || + service_event_type == PlayerActionClient::ServiceEventType::RESPONSE_RECEIVED) + { + return true; + } + + // The request sent comes from the server or client side matches the user setting. + if (play_options_.service_requests_source == ServiceRequestsSource::SERVER_INTROSPECTION) { + if (service_event_type == PlayerActionClient::ServiceEventType::REQUEST_SENT) { + // Ignore request message from service client + return true; + } + } else { + // ServiceRequestsSource::CLIENT_INTROSPECTION + if (service_event_type == PlayerActionClient::ServiceEventType::REQUEST_RECEIVED) { + // Ignore request message from service server + return true; + } + } + + try { + if (!action_client->generic_client()->wait_for_action_server( + std::chrono::milliseconds(300))) + { + RCLCPP_ERROR( + owner_->get_logger(), "The action server '%s' isn't ready !", + action_client->get_action_name().c_str()); + return false; + } + try { // Calling on play message pre-callbacks run_play_msg_pre_callbacks(message); - pre_callbacks_failed = false; } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(owner_->get_logger(), - "Failed to call on play message pre-callback on '" << message->topic_name << + "Failed to call on play action request pre-callback on '" << message->topic_name << "' topic. \nError: " << e.what()); } - if (!pre_callbacks_failed) { - try { - pub_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data)); - message_published = true; - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM(owner_->get_logger(), - "Failed to publish message on '" << message->topic_name << - "' topic. \nError: " << e.what()); - } + switch (action_interface_type) { + case rosbag2_cpp::ActionInterfaceType::SendGoalEvent: + action_client->async_send_goal_request(type_erased_service_event); + break; + case rosbag2_cpp::ActionInterfaceType::CancelGoalEvent: + action_client->async_send_cancel_request(type_erased_service_event); + break; + case rosbag2_cpp::ActionInterfaceType::GetResultEvent: + action_client->async_send_result_request(type_erased_service_event); + break; + + // Never go here + case rosbag2_cpp::ActionInterfaceType::Feedback: + case rosbag2_cpp::ActionInterfaceType::Status: + default: + throw std::logic_error("Wrong action interface type."); } try { @@ -1449,92 +1822,48 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr "Failed to call on play message post-callback on '" << message->topic_name << "' topic. \nError: " << e.what()); } - return message_published; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to send action request for '" << + action_client->get_action_name() << "' action. \nError: " << e.what()); + return false; + } + + return true; +} + +bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) +{ + auto pub_iter = publishers_.find(message->topic_name); + if (pub_iter != publishers_.end()) { + return publish_message_by_player_publisher(pub_iter->second, message); } // Try to publish message as service request auto client_iter = service_clients_.find(message->topic_name); if (play_options_.publish_service_requests && client_iter != service_clients_.end()) { - const auto & service_client = client_iter->second; - auto service_event = service_client->deserialize_service_event(*message->serialized_data); - if (!service_event) { - RCLCPP_ERROR_STREAM( - owner_->get_logger(), "Failed to deserialize service event message for '" << - service_client->get_service_name() << "' service!\n"); - return false; - } - - try { - auto [service_event_type, client_gid] = - service_client->get_service_event_type_and_client_gid(service_event); - // Ignore response message - if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || - service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) - { - return false; - } - - if (play_options_.service_requests_source == ServiceRequestsSource::SERVICE_INTROSPECTION && - service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) - { - return false; - } - - if (play_options_.service_requests_source == ServiceRequestsSource::CLIENT_INTROSPECTION && - service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) - { - return false; - } - - if (!service_client->generic_client()->service_is_ready()) { - RCLCPP_ERROR( - owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", - service_client->get_service_name().c_str()); - return false; - } + return publish_message_by_player_service_client(client_iter->second, message); + } - if (!service_client->is_service_event_include_request_message(service_event)) { - if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { - RCUTILS_LOG_WARN_ONCE_NAMED( - ROSBAG2_TRANSPORT_PACKAGE_NAME, - "Can't send service request. " - "The configuration of introspection for '%s' was metadata only on service side!", - service_client->get_service_name().c_str()); - } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { - RCUTILS_LOG_WARN_ONCE_NAMED( - ROSBAG2_TRANSPORT_PACKAGE_NAME, - "Can't send service request. " - "The configuration of introspection for '%s' client [ID: %s]` was metadata only!", - service_client->get_service_name().c_str(), - rosbag2_cpp::client_id_to_string(client_gid).c_str()); - } - return false; - } - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - owner_->get_logger(), "Failed to send request on '" << - rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << - "' service. \nError: " << e.what()); - return false; + // Try to publish message as action request + auto action_interface_type = rosbag2_cpp::get_action_interface_type(message->topic_name); + if (action_interface_type != rosbag2_cpp::ActionInterfaceType::Unknown) { + if (action_interface_type == rosbag2_cpp::ActionInterfaceType::Feedback || + action_interface_type == rosbag2_cpp::ActionInterfaceType::Status) + { + // Ignore messages for feedback and status + return true; } - // Calling on play message pre-callbacks - run_play_msg_pre_callbacks(message); - - bool message_published = false; - try { - service_client->async_send_request(service_event); - message_published = true; - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - owner_->get_logger(), "Failed to send request on '" << - rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << - "' service. \nError: " << e.what()); + auto action_name = rosbag2_cpp::action_interface_name_to_action_name(message->topic_name); + auto action_client_iter = action_clients_.find(action_name); + if (action_client_iter != action_clients_.end()) { + return publish_message_by_play_action_client(action_client_iter->second, + action_interface_type, message); + } else { + // Ignored action message + return true; } - - // Calling on play message post-callbacks - run_play_msg_post_callbacks(message); - return message_published; } RCUTILS_LOG_WARN_ONCE_NAMED( @@ -1970,6 +2299,17 @@ std::unordered_mapget_service_clients(); } +std::unordered_map> +Player::get_action_clients() +{ + return pimpl_->get_action_clients(); +} + +bool +Player::goal_handle_in_process(std::string action_name, const rclcpp_action::GoalUUID & goal_id) +{ + return pimpl_->goal_handle_in_process(action_name, goal_id); +} rclcpp::Publisher::SharedPtr Player::get_clock_publisher() { return pimpl_->get_clock_publisher(); diff --git a/rosbag2_transport/src/rosbag2_transport/player_action_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_action_client.cpp new file mode 100644 index 0000000000..e430f321aa --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_action_client.cpp @@ -0,0 +1,491 @@ +// 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 "rosbag2_transport/player_action_client.hpp" + +#include "rosbag2_cpp/action_utils.hpp" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +#include "logging.hpp" + +using namespace std::chrono_literals; + +namespace rosbag2_transport +{ + +PlayerActionClient::PlayerActionClient( + rclcpp_action::GenericClient::SharedPtr generic_client, + std::string & action_name, + const std::string & action_type, + rclcpp::Logger logger) +: client_(std::move(generic_client)), + action_name_(action_name), + action_type_(action_type), + logger_(std::move(logger)) +{ + action_ts_lib_ = + rclcpp::get_typesupport_library(action_type, "rosidl_typesupport_cpp"); + + auto action_typesupport_handle = rclcpp::get_action_typesupport_handle( + action_type, "rosidl_typesupport_cpp", *action_ts_lib_); + + auto goal_service_request_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->goal_service_type_support->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + goal_service_request_type_members_ = + static_cast(goal_service_request_type_support_intro->data); + if (goal_service_request_type_members_ == nullptr) { + throw std::invalid_argument( + "goal_service_request_type_members_ for `" + action_name_ + "` is nullptr"); + } + + goal_service_event_type_ts_ = + action_typesupport_handle->goal_service_type_support->event_typesupport; + auto goal_service_event_type_support_intro = get_message_typesupport_handle( + goal_service_event_type_ts_, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + goal_service_event_type_members_ = + static_cast(goal_service_event_type_support_intro->data); + if (goal_service_event_type_members_ == nullptr) { + throw std::invalid_argument( + "goal_service_event_type_members_ for `" + action_name_ + "` is nullptr"); + } + if (goal_service_event_type_members_->member_count_ != 3) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + std::stringstream ss; + ss << "Expected 3 fields in the goal service introspection message, but got " << + goal_service_event_type_members_->member_count_; + throw std::invalid_argument(ss.str()); + } + + auto cancel_service_request_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->cancel_service_type_support->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + cancel_service_request_type_members_ = + static_cast(cancel_service_request_type_support_intro->data); + if (cancel_service_request_type_members_ == nullptr) { + throw std::invalid_argument( + "cancel_service_request_type_members_ for `" + action_name_ + "` is nullptr"); + } + + cancel_service_event_type_ts_ = + action_typesupport_handle->cancel_service_type_support->event_typesupport; + auto cancel_service_event_type_support_intro = get_message_typesupport_handle( + cancel_service_event_type_ts_, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + cancel_service_event_type_members_ = + static_cast(cancel_service_event_type_support_intro->data); + if (cancel_service_event_type_members_ == nullptr) { + throw std::invalid_argument( + "cancel_service_event_type_members_ for `" + action_name_ + "` is nullptr"); + } + if (cancel_service_event_type_members_->member_count_ != 3) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + std::stringstream ss; + ss << "Expected 3 fields in the cancel service introspection message, but got " << + cancel_service_event_type_members_->member_count_; + throw std::invalid_argument(ss.str()); + } + + auto result_service_request_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->result_service_type_support->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + result_service_request_type_members_ = + static_cast(result_service_request_type_support_intro->data); + if (result_service_request_type_members_ == nullptr) { + throw std::invalid_argument( + "result_service_request_type_members_ for `" + action_name_ + "` is nullptr"); + } + + result_service_event_type_ts_ = + action_typesupport_handle->result_service_type_support->event_typesupport; + auto result_service_event_type_support_intro = get_message_typesupport_handle( + result_service_event_type_ts_, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + result_service_event_type_members_ = + static_cast(result_service_event_type_support_intro->data); + if (result_service_event_type_members_ == nullptr) { + throw std::invalid_argument( + "result_service_event_type_members_ for `" + action_name_ + "` is nullptr"); + } + if (result_service_event_type_members_->member_count_ != 3) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + std::stringstream ss; + ss << "Expected 3 fields in the result service introspection message, but got " << + result_service_event_type_members_->member_count_; + throw std::invalid_argument(ss.str()); + } +} + +PlayerActionClient::~PlayerActionClient() +{ + { + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + goal_id_to_goal_handle_map_.clear(); + } + client_->async_cancel_all_goals(); + + { + std::lock_guard lock(goal_ids_to_postpone_send_cancel_mutex_); + goal_ids_to_postpone_send_cancel_.clear(); + } + + { + std::lock_guard lock(goal_ids_to_postpone_send_result_mutex_); + goal_ids_to_postpone_send_result_.clear(); + } +} + +const std::string & PlayerActionClient::get_action_name() +{ + return action_name_; +} + +std::shared_ptr +PlayerActionClient::deserialize_service_event( + const rosidl_message_type_support_t * service_event_type_support, + IntrospectionMessageMembersPtr service_event_members, + const rcl_serialized_message_t & message) +{ + auto type_erased_service_event = std::shared_ptr( + new uint8_t[service_event_members->size_of_], + [fini_function = service_event_members->fini_function](uint8_t * msg) { + fini_function(msg); + delete[] msg; + }); + + service_event_members->init_function( + type_erased_service_event.get(), rosidl_runtime_cpp::MessageInitialization::ZERO); + + rmw_ret_t ret = + rmw_deserialize(&message, service_event_type_support, type_erased_service_event.get()); + if (ret != RMW_RET_OK) { // Failed to deserialize service event message + type_erased_service_event.reset(); + } + return type_erased_service_event; +} + +std::shared_ptr +PlayerActionClient::deserialize_send_goal_service_event(const rcl_serialized_message_t & message) +{ + return deserialize_service_event( + goal_service_event_type_ts_, goal_service_event_type_members_, message); +} + +std::shared_ptr +PlayerActionClient::deserialize_cancel_goal_service_event(const rcl_serialized_message_t & message) +{ + return deserialize_service_event( + cancel_service_event_type_ts_, cancel_service_event_type_members_, message); +} + +std::shared_ptr +PlayerActionClient::deserialize_get_result_service_event(const rcl_serialized_message_t & message) +{ + return deserialize_service_event( + result_service_event_type_ts_, result_service_event_type_members_, message); +} + +PlayerActionClient::ServiceEventType +PlayerActionClient::get_service_event_type( + const std::shared_ptr & type_erased_service_event, + ServiceInterfaceInAction service_type_in_action) +{ + IntrospectionMessageMembersPtr service_event_type_members; + switch (service_type_in_action) { + case ServiceInterfaceInAction::SEND_GOAL_SERVICE: + service_event_type_members = goal_service_event_type_members_; + break; + case ServiceInterfaceInAction::CANCEL_GOAL_SERVICE: + service_event_type_members = cancel_service_event_type_members_; + break; + case ServiceInterfaceInAction::GET_RESULT_SERVICE: + service_event_type_members = result_service_event_type_members_; + break; + } + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & service_info_member = service_event_type_members->members_[0]; + auto service_event_info_ptr = + reinterpret_cast( + type_erased_service_event.get() + service_info_member.offset_); + + switch (service_event_info_ptr->event_type) { + case service_msgs::msg::ServiceEventInfo::REQUEST_SENT: + return ServiceEventType::REQUEST_SENT; + case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: + return ServiceEventType::REQUEST_RECEIVED; + case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: + return ServiceEventType::RESPONSE_SENT; + case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: + return ServiceEventType::RESPONSE_RECEIVED; + default: + // Never go here + throw std::out_of_range("Invalid service event type"); + } +} + +bool +PlayerActionClient::is_request_service_event( + const std::shared_ptr & type_erased_service_event, + IntrospectionMessageMembersPtr service_event_members) +{ + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = service_event_members->members_[0]; + auto service_event_info_ptr = + reinterpret_cast( + type_erased_service_event.get() + request_member.offset_); + + if (service_event_info_ptr->event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT || + service_event_info_ptr->event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + return true; + } + + return false; +} + +void +PlayerActionClient::async_send_goal_request( + const std::shared_ptr & type_erased_send_goal_service_event) +{ + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + + // Confirm that the service event type is REQUEST_SENT or REQUEST_RECEIVED. + if (!is_request_service_event( + type_erased_send_goal_service_event, goal_service_event_type_members_)) + { + return; + } + + const auto & request_member = goal_service_event_type_members_->members_[1]; + void * request_sequence_ptr = type_erased_send_goal_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + void * request_ptr = request_member.get_function(request_sequence_ptr, 0); + + auto send_goal_options = rclcpp_action::GenericClient::SendGoalOptions(); + auto goal_response_callback = + [this](rclcpp_action::GenericClientGoalHandle::SharedPtr goal_handle) { + // Only goal is accepted, then register the goal handle + if (goal_handle) { + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + auto goal_id = goal_handle->get_goal_id(); + + // Check if the Goal ID needs to be canceled. + bool need_to_cancel = false; + { + std::lock_guard lock(goal_ids_to_postpone_send_cancel_mutex_); + if (goal_ids_to_postpone_send_cancel_.count(goal_id) > 0) { + goal_ids_to_postpone_send_cancel_.erase(goal_id); + need_to_cancel = true; + } + } + if (need_to_cancel) { + client_->async_cancel_goal(goal_handle); + // If the goal id also exists in goal_ids_to_postpone_send_result, remove it. + { + std::lock_guard lock(goal_ids_to_postpone_send_result_mutex_); + if (goal_ids_to_postpone_send_result_.count(goal_id) > 0) { + goal_ids_to_postpone_send_result_.erase(goal_id); + } + } + // The goal id need not to be add to goal_id_to_goal_handle_map_. + return; + } + + // Check if the Goal ID needs to get result. + bool need_to_get_result = false; + { + std::lock_guard lock(goal_ids_to_postpone_send_result_mutex_); + if (goal_ids_to_postpone_send_result_.count(goal_id) > 0) { + goal_ids_to_postpone_send_result_.erase(goal_id); + need_to_get_result = true; + } + } + if (need_to_get_result) { + client_->async_get_result(goal_handle); + // The goal id need not to be add to goal_id_to_goal_handle_map_. + return; + } + + goal_id_to_goal_handle_map_[goal_id] = goal_handle; + if (goal_id_to_goal_handle_map_.size() > maximum_goal_handle_size_) { + RCLCPP_WARN( + logger_, + "For action \"%s\", the number of goal handles is over %lu, which may " + "cause memory leak.", action_name_.c_str(), maximum_goal_handle_size_); + } + } + }; + auto result_callback = + [this](const rclcpp_action::GenericClientGoalHandle::WrappedResult & result) { + // Regardless of the return result, this Goal ID has already been completed + // So remove corresponding goal handle. + auto goal_id = result.goal_id; + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + goal_id_to_goal_handle_map_.erase(goal_id); + }; + send_goal_options.goal_response_callback = goal_response_callback; + send_goal_options.result_callback = result_callback; + + client_->async_send_goal(request_ptr, send_goal_options); + } else { + // No service request in the service event. + RCLCPP_WARN( + logger_, + "Can't send goal request since the configuration of introspection for '%s' " + "action was metadata !", action_name_.c_str()); + } +} + +bool +PlayerActionClient::get_goal_id_from_cancel_goal_service_event( + const std::shared_ptr & type_erased_cancel_goal_service_event, GoalID & goal_id) +{ + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = cancel_service_event_type_members_->members_[1]; + void * request_sequence_ptr = + type_erased_cancel_goal_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + void * request_ptr = request_member.get_function(request_sequence_ptr, 0); + auto cancel_request = static_cast(request_ptr); + goal_id = cancel_request->goal_info.goal_id.uuid; + return true; + } else { + // No request data + return false; + } +} + +bool +PlayerActionClient::get_goal_id_from_get_result_service_event( + const std::shared_ptr & type_erased_get_result_service_event, GoalID & goal_id) +{ + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = result_service_event_type_members_->members_[1]; + void * request_sequence_ptr = type_erased_get_result_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + void * request_ptr = request_member.get_function(request_sequence_ptr, 0); + auto result_request = static_cast(request_ptr); + goal_id = *result_request; + return true; + } else { + // No request data + return false; + } +} + +void +PlayerActionClient::async_send_cancel_request( + const std::shared_ptr & type_erased_cancel_goal_service_event) +{ + // Confirm that the service event type is REQUEST_SENT or REQUEST_RECEIVED. + if (!is_request_service_event( + type_erased_cancel_goal_service_event, cancel_service_event_type_members_)) + { + return; + } + + GoalID goal_id; + if (!get_goal_id_from_cancel_goal_service_event( + type_erased_cancel_goal_service_event, goal_id)) + { + RCLCPP_WARN( + logger_, + "Can't send cancel goal request since the configuration of introspection for '%s' " + "action was metadata !", action_name_.c_str()); + } + + rclcpp_action::GenericClientGoalHandle::SharedPtr goal_handle; + { + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + auto it = goal_id_to_goal_handle_map_.find(goal_id); + if (it != goal_id_to_goal_handle_map_.end()) { + goal_handle = it->second; + // Remove this goal handle since goal is going to be canceled + goal_id_to_goal_handle_map_.erase(it); + } else { + // Record this Goal ID, and process the cancel request after the goal is accepted + { + std::lock_guard lock(goal_ids_to_postpone_send_cancel_mutex_); + goal_ids_to_postpone_send_cancel_.insert(goal_id); + } + RCLCPP_DEBUG( + logger_, + "For action \"%s\", postpone sending cancel_goal request since the goal " + "may not be accepted yet.", action_name_.c_str()); + } + } + + client_->async_cancel_goal(goal_handle); +} + +void +PlayerActionClient::async_send_result_request( + const std::shared_ptr & type_erased_get_result_service_event) +{ + // Confirm that the service event type is REQUEST_SENT or REQUEST_RECEIVED. + if (!is_request_service_event( + type_erased_get_result_service_event, result_service_event_type_members_)) + { + return; + } + + GoalID goal_id; + if (!get_goal_id_from_get_result_service_event(type_erased_get_result_service_event, goal_id)) { + RCLCPP_WARN( + logger_, + "Can't send result request since the configuration of introspection for '%s' " + "action was metadata !", action_name_.c_str()); + } + + rclcpp_action::GenericClientGoalHandle::SharedPtr goal_handle; + { + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + auto it = goal_id_to_goal_handle_map_.find(goal_id); + if (it != goal_id_to_goal_handle_map_.end()) { + goal_handle = it->second; + } else { + // Record this Goal ID, and process the result request after the goal is accepted + { + std::lock_guard lock(goal_ids_to_postpone_send_result_mutex_); + goal_ids_to_postpone_send_result_.insert(goal_id); + } + RCLCPP_DEBUG( + logger_, + "For action \"%s\", postpone sending get result request since the goal " + "may not be accepted yet.", action_name_.c_str()); + } + } + + auto result_callback = + [this, goal_id](const rclcpp_action::GenericClientGoalHandle::WrappedResult & result) { + (void) result; + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + goal_id_to_goal_handle_map_.erase(goal_id); + }; + + client_->async_get_result(goal_handle, result_callback); +} + +bool PlayerActionClient::gold_handle_in_processing(const GoalID & goal_id) +{ + std::lock_guard lock(goal_id_to_goal_handle_map_mutex_); + return goal_id_to_goal_handle_map_.count(goal_id) > 0; +} +} // namespace rosbag2_transport diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index 78886a23f7..f27cef28a4 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -6,9 +6,11 @@ player_params_node: rate: 13.0 topics_to_filter: ["/foo", "/bar"] services_to_filter: ["/service1", "/service2"] + action_to_filter: ["/action1", "/action2"] regex_to_filter: "[xyz]/topic_service" exclude_topics_to_filter: ["/exclude_foo", "/exclude_bar"] exclude_services_to_filter: ["/exclude_service1", "/exclude_service2"] + exclude_actions_to_filter: ["/exclude_action1", "/exclude_action2"] exclude_regex_to_filter: "[abc]/topic_service" loop: false clock_publish_frequency: 19.0 @@ -38,6 +40,7 @@ player_params_node: nsec: -999999999 disable_loan_message: false publish_service_requests: false + send_actions_as_client: false service_requests_source: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION message_order: "SENT_TIMESTAMP" # RECEIVED_TIMESTAMP or SENT_TIMESTAMP progress_bar_update_rate: -1 # times per second (Hz) diff --git a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp index 4df421a3e7..961ce4f1e6 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp @@ -20,6 +20,8 @@ #include #include +#include "rclcpp_action/rclcpp_action.hpp" + #include "rosbag2_cpp/reader.hpp" #include "rosbag2_storage/storage_options.hpp" #include "rosbag2_transport/player.hpp" @@ -63,7 +65,7 @@ class MockPlayer : public rosbag2_transport::Player return pub_list; } - std::vector get_list_of_clients() + std::vector get_list_of_service_clients() { std::vector cli_list; for (const auto & client : this->get_service_clients()) { @@ -75,6 +77,26 @@ class MockPlayer : public rosbag2_transport::Player return cli_list; } + std::vector get_list_of_action_clients() + { + std::vector action_cli_list; + for (const auto & client : this->get_action_clients()) { + action_cli_list.push_back( + static_cast( + client.second.get())); + } + + return action_cli_list; + } + + // Before calling this function, make sure that goal has been accepted by the action server. + // Call this function to confirm whether the result response has been received or if a cancel + // request has been sent. + bool goal_handle_complete(std::string action_name, const rclcpp_action::GoalUUID & goal_id) + { + return !this->goal_handle_in_process(action_name, goal_id); + } + size_t get_number_of_registered_pre_callbacks() { return get_number_of_registered_on_play_msg_pre_callbacks(); diff --git a/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp b/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp index ebd314315f..f41a37a3ae 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp @@ -63,6 +63,14 @@ class MockSequentialReader : public rosbag2_cpp::reader_interfaces::BaseReaderIn } } } + + if (!filter_.actions_interfaces.empty()) { + for (const auto & filter_action : filter_.actions_interfaces) { + if (!messages_[num_read_]->topic_name.compare(filter_action)) { + return true; + } + } + } num_read_++; } return false; diff --git a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp index 7b429e7140..6d2f877d21 100644 --- a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp +++ b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp @@ -17,6 +17,7 @@ #include +#include "rosbag2_test_common/action_server_manager.hpp" #include "rosbag2_test_common/subscription_manager.hpp" #include "rosbag2_test_common/service_manager.hpp" #include "rosbag2_transport_test_fixture.hpp" @@ -30,6 +31,7 @@ class RosBag2PlayTestFixture : public Rosbag2TransportTestFixture rclcpp::init(0, nullptr); sub_ = std::make_shared(); srv_ = std::make_shared(); + action_server_ = std::make_shared(); } ~RosBag2PlayTestFixture() override @@ -39,6 +41,7 @@ class RosBag2PlayTestFixture : public Rosbag2TransportTestFixture std::shared_ptr sub_; std::shared_ptr srv_; + std::shared_ptr action_server_; }; #endif // ROSBAG2_TRANSPORT__ROSBAG2_PLAY_TEST_FIXTURE_HPP_ diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index 0d28d86cc5..2aef31c24a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -22,6 +22,7 @@ #include "mock_player.hpp" #include "rosbag2_play_test_fixture.hpp" +#include "test_msgs/action/fibonacci.hpp" #include "test_msgs/message_fixtures.hpp" #include "test_msgs/msg/arrays.hpp" #include "test_msgs/msg/basic_types.hpp" @@ -61,6 +62,62 @@ get_service_event_message_basic_types() return messages; } + +// SendGoal Service Event +static inline std::vector +get_action_event_message_fibonacci_send_goal() +{ + std::vector messages; + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_SendGoal_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + request.goal.order = 10; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_SendGoal_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + request.goal.order = 20; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} + +// GetResult Service Event +static inline std::vector +get_action_event_message_fibonacci_get_result() +{ + std::vector messages; + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_GetResult_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_GetResult_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} } // namespace TEST_F(RosBag2PlayTestFixture, burst_with_false_preconditions) { @@ -465,3 +522,108 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_services) { EXPECT_EQ(service1_receive_requests.size(), 0); EXPECT_EQ(service2_receive_requests.size(), 2); } + +TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_actions) { + const std::string action_name1 = "/test_action1"; + const std::string action_name2 = "/test_action2"; + + size_t action1_request_count = 0; + size_t action1_cancel_count = 0; + action_server_->setup_action( + action_name1, action1_request_count, action1_cancel_count); + + size_t action2_request_count = 0; + size_t action2_cancel_count = 0; + action_server_->setup_action( + action_name2, action2_request_count, action2_cancel_count); + + action_server_->run_action_servers(); + + const std::vector service_event_name1 = { + action_name1 + "/_action/send_goal/_service_event", + action_name1 + "/_action/get_result/_service_event", + action_name1 + "/_action/cancel/_service_event", + action_name1 + "/_action/feedback", + action_name1 + "/_action/status" + }; + const std::vector service_event_name2 = { + action_name2 + "/_action/send_goal/_service_event", + action_name2 + "/_action/get_result/_service_event", + action_name2 + "/_action/cancel/_service_event", + action_name2 + "/_action/feedback", + action_name2 + "/_action/status" + }; + + auto actions_types = std::vector{ + {1u, service_event_name1[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {2u, service_event_name1[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {3u, service_event_name2[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {4u, service_event_name2[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""} + }; + + std::vector> messages = + { + serialize_test_message( + service_event_name1[0], 500, get_action_event_message_fibonacci_send_goal()[0]), + serialize_test_message( + service_event_name2[0], 600, get_action_event_message_fibonacci_send_goal()[1]), + serialize_test_message( + service_event_name2[1], 700, get_action_event_message_fibonacci_get_result()[1]), + serialize_test_message( + service_event_name1[1], 800, get_action_event_message_fibonacci_get_result()[0]) + }; + + // Filter allows action2, blocks action1 + play_options_.actions_to_filter = {action_name2}; + play_options_.send_actions_as_client = true; + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, actions_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + player->pause(); + ASSERT_TRUE(player->is_paused()); + + // Check action servers are ready + ASSERT_TRUE(action_server_->all_action_servers_ready()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + player->play(); + + const size_t EXPECTED_BURST_COUNT = 2; + ASSERT_EQ(player->burst(EXPECTED_BURST_COUNT), EXPECTED_BURST_COUNT); + + ASSERT_TRUE(player->is_paused()); + player->resume(); + + player->wait_for_playback_to_finish(); + + // Wait for getting the result response from action server + std::this_thread::sleep_for(1s); + + exec.cancel(); + spin_thread.join(); + + EXPECT_EQ(action1_request_count, 0); + EXPECT_EQ(action1_cancel_count, 0); + EXPECT_EQ(action2_request_count, 1); + EXPECT_EQ(action2_cancel_count, 0); + + // Check if only one action client was created. + auto action_client_list = player->get_list_of_action_clients(); + ASSERT_THAT(action_client_list, SizeIs(1)); + + // There is no direct interface to confirm that rosbag2 received the get_result response. + // Here, the logic of handling actions in rosbag2 is used to make this determination. If + // the action server receives a goal, the corresponding goal_handle in rosbag will be cleared + // in two scenarios: one is after a cancel request is sent, and the other is when the get_result + // is received. + // In this testcase, the cancel request is not sent, so the goal_handle should be cleared after + // the get_result is received. + EXPECT_TRUE( + player->goal_handle_complete( + action_name2, + get_action_event_message_fibonacci_send_goal()[1]->request[0].goal_id.uuid)); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index 83676cb6bd..fc4d525ae2 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -147,6 +147,9 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { "/service1/_service_event", "/service2/_service_event"}; EXPECT_EQ(play_options.services_to_filter, services_to_filter); + std::vector actions_to_filter { + "/action1", + "/action2"}; EXPECT_EQ(play_options.regex_to_filter, "[xyz]/topic_service"); std::vector exclude_topics_to_filter {"/exclude_foo", "/exclude_bar"}; EXPECT_EQ(play_options.exclude_topics_to_filter, exclude_topics_to_filter); @@ -154,6 +157,10 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { "/exclude_service1/_service_event", "/exclude_service2/_service_event"}; EXPECT_EQ(play_options.exclude_services_to_filter, exclude_services_to_filter); + std::vector exclude_actions_to_filter { + "/exclude_action1", + "/exclude_action2"}; + EXPECT_EQ(play_options.exclude_actions_to_filter, exclude_actions_to_filter); EXPECT_EQ(play_options.exclude_regex_to_filter, "[abc]/topic_service"); std::unordered_map topic_qos_profile_overrides{ @@ -174,6 +181,7 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { EXPECT_EQ(play_options.wait_acked_timeout, -999999999); EXPECT_EQ(play_options.disable_loan_message, false); EXPECT_EQ(play_options.publish_service_requests, false); + EXPECT_EQ(play_options.send_actions_as_client, false); EXPECT_EQ( play_options.service_requests_source, rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 5c3fb51f47..90316f186c 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -24,11 +24,13 @@ #include "rclcpp/rclcpp.hpp" -#include "rosbag2_test_common/subscription_manager.hpp" +#include "rosbag2_test_common/action_server_manager.hpp" #include "rosbag2_test_common/service_manager.hpp" +#include "rosbag2_test_common/subscription_manager.hpp" #include "rosbag2_transport/player.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" @@ -107,6 +109,229 @@ void spin_thread_and_wait_for_sent_service_requests_to_finish( exec.cancel(); if (spin_thread.joinable()) {spin_thread.join();} } + +// SendGoal Service Event +static inline std::vector +get_action_event_message_fibonacci_send_goal() +{ + std::vector messages; + + // action 1 (from action server) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_SendGoal_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + request.goal.order = 2; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + // action 2 (from action server) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_SendGoal_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + request.goal.order = 3; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + // action 1 (from action client) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::action::Fibonacci_SendGoal_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + request.goal.order = 2; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} + +// GetResult Service Event +static inline std::vector +get_action_event_message_fibonacci_get_result() +{ + std::vector messages; + + // action 1 (from action server) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_GetResult_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + // action 2 (from action server) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::action::Fibonacci_GetResult_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + // action 1 (from action client) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::action::Fibonacci_GetResult_Request request; + request.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} + +// CancelGoal Service Event +static inline std::vector +get_action_event_message_fibonacci_Cancel_Goal() +{ + std::vector messages; + + // action 1 (from action server) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + action_msgs::srv::CancelGoal_Request request; + request.goal_info.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + // action 2 (from action server) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + action_msgs::srv::CancelGoal_Request request; + request.goal_info.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + // action 1 (from action client) + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + action_msgs::srv::CancelGoal_Request request; + request.goal_info.goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} + +// feedback, these messages will be ignored while setting --send-actions-as-client +static inline std::vector +get_action_message_fibonacci_feedback() +{ + std::vector messages; + + // action 1 + { + auto msg = std::make_shared(); + msg->goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->feedback.sequence = {1, 2}; + messages.push_back(msg); + } + + // action 2 + { + auto msg = std::make_shared(); + msg->goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->feedback.sequence = {1, 2}; + messages.push_back(msg); + } + + // action 2 + { + auto msg = std::make_shared(); + msg->goal_id.uuid = {1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->feedback.sequence = {1, 2, 3}; + messages.push_back(msg); + } + + return messages; +} + +// status, these messages will be ignored while setting --send-actions-as-client +static inline std::vector +get_action_message_fibonacci_status() +{ + std::vector messages; + + // action1 accepted + { + auto msg = std::make_shared(); + msg->status_list.resize(1); + msg->status_list[0].goal_info.goal_id.uuid = { + 1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->status_list[0].status = action_msgs::msg::GoalStatus::STATUS_ACCEPTED; + messages.push_back(msg); + } + + // action1 canceled + { + auto msg = std::make_shared(); + msg->status_list.resize(1); + msg->status_list[0].goal_info.goal_id.uuid = { + 1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->status_list[0].status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + messages.push_back(msg); + } + + // action1 succeeded + { + auto msg = std::make_shared(); + msg->status_list.resize(1); + msg->status_list[0].goal_info.goal_id.uuid = { + 1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 4}; + msg->status_list[0].status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + messages.push_back(msg); + } + + // action2 accepted + { + auto msg = std::make_shared(); + msg->status_list.resize(1); + msg->status_list[0].goal_info.goal_id.uuid = { + 1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->status_list[0].status = action_msgs::msg::GoalStatus::STATUS_ACCEPTED; + messages.push_back(msg); + } + + // action2 canceled + { + auto msg = std::make_shared(); + msg->status_list.resize(1); + msg->status_list[0].goal_info.goal_id.uuid = { + 1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->status_list[0].status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + messages.push_back(msg); + } + + // action2 succeeded + { + auto msg = std::make_shared(); + msg->status_list.resize(1); + msg->status_list[0].goal_info.goal_id.uuid = { + 1, 15, 147, 28, 42, 149, 174, 3, 0, 0, 0, 0, 0, 0, 20, 5}; + msg->status_list[0].status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + messages.push_back(msg); + } + + return messages; +} + } // namespace class RosBag2PlayTestFixtureMessageOrder @@ -305,6 +530,391 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_services) EXPECT_EQ(service2_receive_requests.size(), 2); } +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_as_action_client_for_all_actions) +{ + const std::string action_name1 = "/test_action1"; + const std::string action_name2 = "/test_action2"; + + size_t action1_request_count = 0; + size_t action1_cancel_count = 0; + action_server_->setup_action( + action_name1, action1_request_count, action1_cancel_count); + + size_t action2_request_count = 0; + size_t action2_cancel_count = 0; + action_server_->setup_action( + action_name2, action2_request_count, action2_cancel_count); + + action_server_->run_action_servers(); + + const std::vector service_event_name1 = { + action_name1 + "/_action/send_goal/_service_event", + action_name1 + "/_action/get_result/_service_event", + action_name1 + "/_action/cancel/_service_event", + action_name1 + "/_action/feedback", + action_name1 + "/_action/status" + }; + const std::vector service_event_name2 = { + action_name2 + "/_action/send_goal/_service_event", + action_name2 + "/_action/get_result/_service_event", + action_name2 + "/_action/cancel/_service_event", + action_name2 + "/_action/feedback", + action_name2 + "/_action/status" + }; + + auto actions_types = std::vector{ + {1u, service_event_name1[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {2u, service_event_name1[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {3u, service_event_name1[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {4u, service_event_name1[4], "action_msgs/msg/GoalStatusArray", "", {}, ""}, + {5u, service_event_name2[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {6u, service_event_name2[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {7u, service_event_name2[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {8u, service_event_name2[4], "action_msgs/msg/GoalStatusArray", "", {}, ""} + }; + + std::vector> messages = + { + // action1 send goal request + serialize_test_message( + service_event_name1[0], 500, get_action_event_message_fibonacci_send_goal()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 530, get_action_message_fibonacci_status()[0]), + // action1 feedback + serialize_test_message( + service_event_name1[3], 540, get_action_message_fibonacci_feedback()[0]), + // action2 send goal request + serialize_test_message( + service_event_name2[0], 600, get_action_event_message_fibonacci_send_goal()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 620, get_action_message_fibonacci_status()[3]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 630, get_action_message_fibonacci_feedback()[1]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 640, get_action_message_fibonacci_feedback()[2]), + // action2 get result response + serialize_test_message( + service_event_name2[1], 700, get_action_event_message_fibonacci_get_result()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 710, get_action_message_fibonacci_status()[5]), + // action1 get result response + serialize_test_message( + service_event_name1[1], 800, get_action_event_message_fibonacci_get_result()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 810, get_action_message_fibonacci_status()[2]), + }; + + // send actions as client + play_options_.send_actions_as_client = true; + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, actions_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + // Check action servers are ready + ASSERT_TRUE(action_server_->all_action_servers_ready()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + player->play(); + + player->wait_for_playback_to_finish(); + + // Wait for getting the result response from action server + std::this_thread::sleep_for(1s); + + exec.cancel(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + + EXPECT_EQ(action1_request_count, 1); + EXPECT_EQ(action1_cancel_count, 0); + EXPECT_EQ(action2_request_count, 1); + EXPECT_EQ(action2_cancel_count, 0); + + // There is no direct interface to confirm that rosbag2 received the get_result response. + // Here, the logic of handling actions in rosbag2 is used to make this determination. If + // the action server receives a goal, the corresponding goal_handle in rosbag will be cleared + // in two scenarios: one is after a cancel request is sent, and the other is when the get_result + // is received. + // In this testcase, the cancel request is not sent, so the goal_handle should be cleared after + // the get_result is received. + EXPECT_TRUE( + player->goal_handle_complete( + action_name1, + get_action_event_message_fibonacci_send_goal()[0]->request[0].goal_id.uuid)); + EXPECT_TRUE( + player->goal_handle_complete( + action_name2, + get_action_event_message_fibonacci_send_goal()[1]->request[0].goal_id.uuid)); +} + +TEST_F(RosBag2PlayTestFixture, + recorded_messages_with_cancel_are_played_as_action_client_for_all_actions) +{ + const std::string action_name1 = "/test_action1"; + const std::string action_name2 = "/test_action2"; + + size_t action1_request_count = 0; + size_t action1_cancel_count = 0; + action_server_->setup_action( + action_name1, action1_request_count, action1_cancel_count, 1s); + + size_t action2_request_count = 0; + size_t action2_cancel_count = 0; + action_server_->setup_action( + action_name2, action2_request_count, action2_cancel_count); + + action_server_->run_action_servers(); + + // Check action servers are ready + ASSERT_TRUE(action_server_->all_action_servers_ready()); + + const std::vector service_event_name1 = { + action_name1 + "/_action/send_goal/_service_event", + action_name1 + "/_action/get_result/_service_event", + action_name1 + "/_action/cancel_goal/_service_event", + action_name1 + "/_action/feedback", + action_name1 + "/_action/status" + }; + const std::vector service_event_name2 = { + action_name2 + "/_action/send_goal/_service_event", + action_name2 + "/_action/get_result/_service_event", + action_name2 + "/_action/cancel_goal/_service_event", + action_name2 + "/_action/feedback", + action_name2 + "/_action/status" + }; + + auto actions_types = std::vector{ + {1u, service_event_name1[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {2u, service_event_name1[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {3u, service_event_name1[2], "action_msgs/srv/CancelGoal_Event", "", {}, ""}, + {4u, service_event_name1[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {5u, service_event_name1[4], "action_msgs/msg/GoalStatusArray", "", {}, ""}, + {6u, service_event_name2[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {7u, service_event_name2[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {8u, service_event_name2[2], "action_msgs/srv/CancelGoal_Event", "", {}, ""}, + {9u, service_event_name2[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {10u, service_event_name2[4], "action_msgs/msg/GoalStatusArray", "", {}, ""} + }; + + std::vector> messages = + { + // action1 send goal request + serialize_test_message( + service_event_name1[0], 100, get_action_event_message_fibonacci_send_goal()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 130, get_action_message_fibonacci_status()[0]), + // action1 feedback + serialize_test_message( + service_event_name1[3], 140, get_action_message_fibonacci_feedback()[0]), + // action2 send goal request + serialize_test_message( + service_event_name2[0], 200, get_action_event_message_fibonacci_send_goal()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 220, get_action_message_fibonacci_status()[3]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 230, get_action_message_fibonacci_feedback()[1]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 240, get_action_message_fibonacci_feedback()[2]), + // action2 get result response + serialize_test_message( + service_event_name2[1], 300, get_action_event_message_fibonacci_get_result()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 710, get_action_message_fibonacci_status()[5]), + // action1 cancel goal request + serialize_test_message( + service_event_name1[2], 500, get_action_event_message_fibonacci_Cancel_Goal()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 500, get_action_message_fibonacci_status()[1]) + }; + + play_options_.send_actions_as_client = true; + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, actions_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + player->play(); + + player->wait_for_playback_to_finish(); + + // Wait for getting the result response from action server + std::this_thread::sleep_for(1500ms); + + exec.cancel(); + spin_thread.join(); + + EXPECT_EQ(action1_request_count, 1); + EXPECT_EQ(action1_cancel_count, 1); + EXPECT_EQ(action2_request_count, 1); + EXPECT_EQ(action2_cancel_count, 0); + + // There is no direct interface to confirm that rosbag2 received the get_result response. + // Here, the logic of handling actions in rosbag2 is used to make this determination. If + // the action server receives a goal, the corresponding goal_handle in rosbag will be cleared + // in two scenarios: one is after a cancel request is sent, and the other is when the get_result + // is received. + // Confirm goal_handles are all removed. + EXPECT_TRUE( + player->goal_handle_complete( + action_name1, + get_action_event_message_fibonacci_send_goal()[0]->request[0].goal_id.uuid)); + EXPECT_TRUE( + player->goal_handle_complete( + action_name2, + get_action_event_message_fibonacci_send_goal()[1]->request[0].goal_id.uuid)); +} + +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_actions) +{ + // All recorded messages of actions are played as normal topic messages + // --send-actions-as-client isn't set + + const std::string action_name1 = "/test_action1"; + const std::string action_name2 = "/test_action2"; + + const std::vector service_event_name1 = { + action_name1 + "/_action/send_goal/_service_event", + action_name1 + "/_action/get_result/_service_event", + action_name1 + "/_action/cancel/_service_event", + action_name1 + "/_action/feedback", + action_name1 + "/_action/status" + }; + const std::vector service_event_name2 = { + action_name2 + "/_action/send_goal/_service_event", + action_name2 + "/_action/get_result/_service_event", + action_name2 + "/_action/cancel/_service_event", + action_name2 + "/_action/feedback", + action_name2 + "/_action/status" + }; + + auto actions_types = std::vector{ + {1u, service_event_name1[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {2u, service_event_name1[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {3u, service_event_name1[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {4u, service_event_name1[4], "action_msgs/msg/GoalStatusArray", "", {}, ""}, + {5u, service_event_name2[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {6u, service_event_name2[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {7u, service_event_name2[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {8u, service_event_name2[4], "action_msgs/msg/GoalStatusArray", "", {}, ""} + }; + + std::vector> messages = + { + // action1 send goal request + serialize_test_message( + service_event_name1[0], 100, get_action_event_message_fibonacci_send_goal()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 130, get_action_message_fibonacci_status()[0]), + // action1 feedback + serialize_test_message( + service_event_name1[3], 140, get_action_message_fibonacci_feedback()[0]), + // action2 send goal request + serialize_test_message( + service_event_name2[0], 200, get_action_event_message_fibonacci_send_goal()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 220, get_action_message_fibonacci_status()[3]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 230, get_action_message_fibonacci_feedback()[1]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 240, get_action_message_fibonacci_feedback()[2]), + // action2 get result response + serialize_test_message( + service_event_name2[1], 300, get_action_event_message_fibonacci_get_result()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 310, get_action_message_fibonacci_status()[5]), + // action1 get result response + serialize_test_message( + service_event_name1[1], 400, get_action_event_message_fibonacci_get_result()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 410, get_action_message_fibonacci_status()[2]), + }; + + sub_->add_subscription(service_event_name1[0], 1); + sub_->add_subscription(service_event_name1[1], 1); + sub_->add_subscription(service_event_name1[3], 1); + sub_->add_subscription(service_event_name1[4], 2); + sub_->add_subscription(service_event_name2[0], 1); + sub_->add_subscription(service_event_name2[1], 1); + sub_->add_subscription(service_event_name2[3], 2); + sub_->add_subscription(service_event_name2[4], 2); + + auto await_received_messages = sub_->spin_subscriptions(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, actions_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + player->play(); + player->wait_for_playback_to_finish(); + + await_received_messages.get(); + + auto replayed_action1_send_goal = + sub_->get_received_messages( + service_event_name1[0]); + EXPECT_THAT(replayed_action1_send_goal, SizeIs(1u)); + + auto replayed_action1_get_result = + sub_->get_received_messages( + service_event_name1[1]); + EXPECT_THAT(replayed_action1_get_result, SizeIs(1u)); + + auto replayed_action1_feedback = + sub_->get_received_messages( + service_event_name1[3]); + EXPECT_THAT(replayed_action1_feedback, SizeIs(1u)); + + auto replayed_action1_status = + sub_->get_received_messages( + service_event_name1[4]); + EXPECT_THAT(replayed_action1_status, SizeIs(2u)); + + auto replayed_action2_send_goal = + sub_->get_received_messages( + service_event_name2[0]); + EXPECT_THAT(replayed_action2_send_goal, SizeIs(1u)); + + auto replayed_action2_get_result = + sub_->get_received_messages( + service_event_name2[1]); + EXPECT_THAT(replayed_action2_get_result, SizeIs(1u)); + + auto replayed_action2_feedback = + sub_->get_received_messages( + service_event_name2[3]); + EXPECT_THAT(replayed_action2_feedback, SizeIs(2u)); + + auto replayed_action2_status = + sub_->get_received_messages( + service_event_name2[4]); + EXPECT_THAT(replayed_action2_status, SizeIs(2u)); +} + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_topics_and_services) { auto topic_msg = get_messages_basic_types()[0]; @@ -665,6 +1275,132 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service } } +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_actions) +{ + // Block test_action1 and allow test_action2 + + const std::string action_name1 = "/test_action1"; + const std::string action_name2 = "/test_action2"; + + size_t action1_request_count = 0; + size_t action1_cancel_count = 0; + action_server_->setup_action( + action_name1, action1_request_count, action1_cancel_count); + + size_t action2_request_count = 0; + size_t action2_cancel_count = 0; + action_server_->setup_action( + action_name2, action2_request_count, action2_cancel_count); + + action_server_->run_action_servers(); + + const std::vector service_event_name1 = { + action_name1 + "/_action/send_goal/_service_event", + action_name1 + "/_action/get_result/_service_event", + action_name1 + "/_action/cancel/_service_event", + action_name1 + "/_action/feedback", + action_name1 + "/_action/status" + }; + const std::vector service_event_name2 = { + action_name2 + "/_action/send_goal/_service_event", + action_name2 + "/_action/get_result/_service_event", + action_name2 + "/_action/cancel/_service_event", + action_name2 + "/_action/feedback", + action_name2 + "/_action/status" + }; + + auto actions_types = std::vector{ + {1u, service_event_name1[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {2u, service_event_name1[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {3u, service_event_name1[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {4u, service_event_name1[4], "action_msgs/msg/GoalStatusArray", "", {}, ""}, + {5u, service_event_name2[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {6u, service_event_name2[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {7u, service_event_name2[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {8u, service_event_name2[4], "action_msgs/msg/GoalStatusArray", "", {}, ""} + }; + + std::vector> messages = + { + // action1 send goal request + serialize_test_message( + service_event_name1[0], 500, get_action_event_message_fibonacci_send_goal()[0]), + // action1 status + serialize_test_message( + service_event_name1[4], 530, get_action_message_fibonacci_status()[0]), + // action1 feedback + serialize_test_message( + service_event_name1[3], 540, get_action_message_fibonacci_feedback()[0]), + // action2 send goal request + serialize_test_message( + service_event_name2[0], 600, get_action_event_message_fibonacci_send_goal()[1]), + // action2 status + serialize_test_message( + service_event_name1[4], 620, get_action_message_fibonacci_status()[3]), + // action2 feedback + serialize_test_message( + service_event_name1[3], 630, get_action_message_fibonacci_feedback()[1]), + // action2 feedback + serialize_test_message( + service_event_name1[3], 640, get_action_message_fibonacci_feedback()[2]), + // action2 get result response + serialize_test_message( + service_event_name2[1], 700, get_action_event_message_fibonacci_get_result()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 710, get_action_message_fibonacci_status()[5]), + // action1 get result response + serialize_test_message( + service_event_name1[1], 800, get_action_event_message_fibonacci_get_result()[0]), + // action1 status + serialize_test_message( + service_event_name2[4], 810, get_action_message_fibonacci_status()[2]), + }; + + // send actions as client + play_options_.actions_to_filter = {action_name2}; + play_options_.send_actions_as_client = true; + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, actions_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + // Check action servers are ready + ASSERT_TRUE(action_server_->all_action_servers_ready()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + player->play(); + + player->wait_for_playback_to_finish(); + + // Wait for getting the result response from action server + std::this_thread::sleep_for(1s); + + exec.cancel(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + + EXPECT_EQ(action1_request_count, 0); + EXPECT_EQ(action1_cancel_count, 0); + ASSERT_EQ(action2_request_count, 1); + EXPECT_EQ(action2_cancel_count, 0); + + // There is no direct interface to confirm that rosbag2 received the get_result response. + // Here, the logic of handling actions in rosbag2 is used to make this determination. If + // the action server receives a goal, the corresponding goal_handle in rosbag will be cleared + // in two scenarios: one is after a cancel request is sent, and the other is when the get_result + // is received. + // Action2 request was accepted, so the goal_handle should be cleared after the get_result is + // received. + EXPECT_TRUE( + player->goal_handle_complete( + action_name2, + get_action_event_message_fibonacci_send_goal()[1]->request[0].goal_id.uuid)); +} + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_and_services) { const std::string topic_name1 = "/topic1"; @@ -1018,7 +1754,7 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_ ASSERT_TRUE(srv_->all_services_ready()); play_options_.publish_service_requests = true; - play_options_.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + play_options_.service_requests_source = ServiceRequestsSource::SERVER_INTROSPECTION; auto player = std::make_shared(std::move(reader), storage_options_, play_options_); @@ -1081,6 +1817,133 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_m } } +TEST_F(RosBag2PlayTestFixture, play_actions_message_from_action_client_introspection_messages) +{ + // action1 messages from action1 client, action2 messages from action2 server + // So only action1 messages can be played as action client + const std::string action_name1 = "/test_action1"; + const std::string action_name2 = "/test_action2"; + + size_t action1_request_count = 0; + size_t action1_cancel_count = 0; + action_server_->setup_action( + action_name1, action1_request_count, action1_cancel_count); + + size_t action2_request_count = 0; + size_t action2_cancel_count = 0; + action_server_->setup_action( + action_name2, action2_request_count, action2_cancel_count); + + action_server_->run_action_servers(); + + const std::vector service_event_name1 = { + action_name1 + "/_action/send_goal/_service_event", + action_name1 + "/_action/get_result/_service_event", + action_name1 + "/_action/cancel/_service_event", + action_name1 + "/_action/feedback", + action_name1 + "/_action/status" + }; + const std::vector service_event_name2 = { + action_name2 + "/_action/send_goal/_service_event", + action_name2 + "/_action/get_result/_service_event", + action_name2 + "/_action/cancel/_service_event", + action_name2 + "/_action/feedback", + action_name2 + "/_action/status" + }; + + auto actions_types = std::vector{ + {1u, service_event_name1[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {2u, service_event_name1[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {3u, service_event_name1[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {4u, service_event_name1[4], "action_msgs/msg/GoalStatusArray", "", {}, ""}, + {5u, service_event_name2[0], "test_msgs/action/Fibonacci_SendGoal_Event", "", {}, ""}, + {6u, service_event_name2[1], "test_msgs/action/Fibonacci_GetResult_Event", "", {}, ""}, + {7u, service_event_name2[3], "test_msgs/action/Fibonacci_FeedbackMessage", "", {}, ""}, + {8u, service_event_name2[4], "action_msgs/msg/GoalStatusArray", "", {}, ""} + }; + + std::vector> messages = + { + // action1 send goal request + serialize_test_message( + service_event_name1[0], 500, get_action_event_message_fibonacci_send_goal()[2]), + // action1 status + serialize_test_message( + service_event_name1[4], 530, get_action_message_fibonacci_status()[0]), + // action1 feedback + serialize_test_message( + service_event_name1[3], 540, get_action_message_fibonacci_feedback()[0]), + // action2 send goal request + serialize_test_message( + service_event_name2[0], 600, get_action_event_message_fibonacci_send_goal()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 620, get_action_message_fibonacci_status()[3]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 630, get_action_message_fibonacci_feedback()[1]), + // action2 feedback + serialize_test_message( + service_event_name2[3], 640, get_action_message_fibonacci_feedback()[2]), + // action2 get result response + serialize_test_message( + service_event_name2[1], 700, get_action_event_message_fibonacci_get_result()[1]), + // action2 status + serialize_test_message( + service_event_name2[4], 710, get_action_message_fibonacci_status()[5]), + // action1 get result response + serialize_test_message( + service_event_name1[1], 800, get_action_event_message_fibonacci_get_result()[2]), + // action1 status + serialize_test_message( + service_event_name1[4], 810, get_action_message_fibonacci_status()[2]), + }; + + // send actions as client + play_options_.send_actions_as_client = true; + // Only play requests from action client + play_options_.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, actions_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + // Check action servers are ready + ASSERT_TRUE(action_server_->all_action_servers_ready()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + player->play(); + + player->wait_for_playback_to_finish(); + + // Wait for getting the result response from action server + std::this_thread::sleep_for(1s); + + exec.cancel(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + + ASSERT_EQ(action1_request_count, 1); + EXPECT_EQ(action1_cancel_count, 0); + EXPECT_EQ(action2_request_count, 0); + EXPECT_EQ(action2_cancel_count, 0); + + // There is no direct interface to confirm that rosbag2 received the get_result response. + // Here, the logic of handling actions in rosbag2 is used to make this determination. If + // the action server receives a goal, the corresponding goal_handle in rosbag will be cleared + // in two scenarios: one is after a cancel request is sent, and the other is when the get_result + // is received. + // In this testcase, the cancel request is not sent, so the goal_handle should be cleared after + // the get_result is received. + EXPECT_TRUE( + player->goal_handle_complete( + action_name1, + get_action_event_message_fibonacci_send_goal()[0]->request[0].goal_id.uuid)); +} + TEST_F(RosBag2PlayTestFixture, play_service_events_and_topics) { const std::string topic_1_name = "/topic1"; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp index ebd0c58a1d..e6b2cad29d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp @@ -69,7 +69,7 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) { auto await_received_messages = sub_->spin_subscriptions(); rosbag2_transport::PlayOptions play_options = { - read_ahead_queue_size, "", rate, {}, {}, "", {}, {}, "", {}, loop_playback, {}, + read_ahead_queue_size, "", rate, {}, {}, {}, "", {}, {}, {}, "", {}, loop_playback, {}, clock_publish_frequency, clock_publish_per_topic, clock_trigger_topics, delay}; auto player = std::make_shared( std::move( @@ -132,8 +132,8 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) { auto await_received_messages = sub_->spin_subscriptions(); - rosbag2_transport::PlayOptions play_options{read_ahead_queue_size, "", rate, {}, {}, "", - {}, {}, "", {}, loop_playback, {}, clock_publish_frequency, clock_publish_per_topic, + rosbag2_transport::PlayOptions play_options{read_ahead_queue_size, "", rate, {}, {}, {}, "", + {}, {}, {}, "", {}, loop_playback, {}, clock_publish_frequency, clock_publish_per_topic, clock_trigger_topics, delay}; auto player = std::make_shared( std::move(