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