Implement rosbag2 action play

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`).
@@ -189,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 <bag>
+```
+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,
@@ -282,12 +295,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

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,7 @@ 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)
+        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

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,13 +285,18 @@ 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
 
         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

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]

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

rosbag2_py/src/rosbag2_py/_storage.cpp

@@ -128,19 +128,23 @@ PYBIND11_MODULE(_storage, m) {
   pybind11::class_<rosbag2_storage::StorageFilter>(m, "StorageFilter")
   .def(
     pybind11::init<
-      std::vector<std::string>, std::vector<std::string>, std::string,
-      std::vector<std::string>, std::vector<std::string>, std::string>(),
+      std::vector<std::string>, std::vector<std::string>, std::vector<std::string>, std::string,
+      std::vector<std::string>, std::vector<std::string>, std::vector<std::string>, std::string>(),
     pybind11::arg("topics") = std::vector<std::string>(),
     pybind11::arg("services_events") = std::vector<std::string>(),
+    pybind11::arg("actions") = std::vector<std::string>(),
     pybind11::arg("regex") = "",
     pybind11::arg("exclude_topics") = std::vector<std::string>(),
     pybind11::arg("exclude_service_events") = std::vector<std::string>(),
+    pybind11::arg("exclude_actions") = std::vector<std::string>(),
     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_<rosbag2_storage::MessageDefinition>(m, "MessageDefinition")

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_<rosbag2_transport::ServiceRequestsSource>(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)
   ;
 

rosbag2_storage/include/rosbag2_storage/storage_filter.hpp

@@ -34,6 +34,12 @@ struct StorageFilter
   // are returned.
   std::vector<std::string> 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<std::string> 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<std::string> 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<std::string> 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

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<std::string> 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 <typename T>
 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<std::string> * include_list = nullptr;
+    std::vector<std::string> * 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;
       }
     }