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