Implement action recording and display info about recorded action

Signed-off-by: Barry Xu <[email protected]>

Author: Barry-Xu-2018

ros2bag/ros2bag/api/__init__.py

@@ -321,3 +321,19 @@ def convert_service_to_service_event_topic(services):
         services_event_topics.append(service + '/_service_event')
 
     return services_event_topics
+
+
+def convert_action_to_all_related_topics(actions):
+    action_topics = []
+
+    if not actions:
+        return action_topics
+
+    for action in actions:
+        action_topics.append(action + '/_action/send_goal/_service_event')
+        action_topics.append(action + '/_action/get_result/_service_event')
+        action_topics.append(action + '/_action/cancel_goal/_service_event')
+        action_topics.append(action + '/_action/status')
+        action_topics.append(action + '/_action/feedback')
+
+    return action_topics

ros2bag/ros2bag/verb/info.py

@@ -28,7 +28,8 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         )
         parser.add_argument(
             '-v', '--verbose', action='store_true',
-            help='Display request/response information for services'
+            help='Display request/response information for services. Display request/response '
+            'information for action internal services'
         )
         available_sorting_methods = Info().get_sorting_methods()
         parser.add_argument(

ros2bag/ros2bag/verb/record.py

@@ -18,6 +18,7 @@
 
 from rclpy.qos import InvalidQoSProfileException
 from ros2bag.api import add_writer_storage_plugin_extensions
+from ros2bag.api import convert_action_to_all_related_topics
 from ros2bag.api import convert_service_to_service_event_topic
 from ros2bag.api import convert_yaml_to_qos_profile
 from ros2bag.api import print_error
@@ -70,27 +71,34 @@ def add_recorder_arguments(parser: ArgumentParser) -> None:
     parser.add_argument(
         '--services', type=str, metavar='ServiceName', nargs='+',
         help='Space-delimited list of services to record.')
+    parser.add_argument(
+        '--actions', type=str, metavar='ActionName', nargs='+',
+        help='Space-delimited list of actions to record.')
     parser.add_argument(
         '--topic-types', nargs='+', default=[], metavar='TopicType',
         help='Space-delimited list of topic types to record.')
     parser.add_argument(
         '-a', '--all', action='store_true',
-        help='Record all topics and services (Exclude hidden topic).')
+        help='Record all topics, services and actions (Exclude hidden topic).')
     parser.add_argument(
         '--all-topics', action='store_true',
         help='Record all topics (Exclude hidden topic).')
     parser.add_argument(
         '--all-services', action='store_true',
         help='Record all services via service event topics.')
+    parser.add_argument(
+        '--all-actions', action='store_true',
+        help='Record all actions via internal topics and service event topics.')
     parser.add_argument(
         '-e', '--regex', default='',
         help='Record only topics and services containing provided regular expression. '
-             'Note:  --all, --all-topics or --all-services will override --regex.')
+             'Note:  --all, --all-topics, --all-services or --all-actions will override --regex.')
     parser.add_argument(
         '--exclude-regex', default='',
         help='Exclude topics and services containing provided regular expression. '
              'Works on top of '
-             '--all, --all-topics, --all-services, --topics, --services or --regex.')
+             '--all, --all-topics, --all-services, --all-actions, --topics, --services, --actions '
+             'or --regex.')
     parser.add_argument(
         '--exclude-topic-types', type=str, default=[], metavar='ExcludeTopicTypes', nargs='+',
         help='Space-delimited list of topic types not being recorded. '
@@ -103,6 +111,10 @@ def add_recorder_arguments(parser: ArgumentParser) -> None:
         '--exclude-services', type=str, metavar='ServiceName', nargs='+',
         help='Space-delimited list of services not being recorded. '
              'Works on top of --all, --all-services, --services or --regex.')
+    parser.add_argument(
+        '--exclude-actions', type=str, metavar='ActionName', nargs='+',
+        help='Space-delimited list of actions not being recorded. '
+             'Works on top of --all, --all-actions, --actions or --regex.')
 
     # Discovery behavior
     parser.add_argument(
@@ -217,10 +229,11 @@ def add_recorder_arguments(parser: ArgumentParser) -> None:
 
 
 def check_necessary_argument(args):
-    # At least one options out of --all, --all-topics, --all-services, --services, --topics,
-    # --topic-types or --regex must be used
-    if not (args.all or args.all_topics or args.all_services or
+    # At least one options out of --all, --all-topics, --all-services, --all-actions, --services,
+    # --actions --topics, --topic-types or --regex must be used
+    if not (args.all or args.all_topics or args.all_services or args.all_actions or
             (args.services and len(args.services) > 0) or
+            (args.actions and len(args.actions) > 0) or
             (args.topics and len(args.topics) > 0) or
             (args.topic_types and len(args.topic_types) > 0) or args.regex):
         return False
@@ -239,9 +252,9 @@ def validate_parsed_arguments(args, uri) -> str:
 
     if args.exclude_regex and not \
             (args.all or args.all_topics or args.topic_types or args.all_services or
-             args.regex):
+             args.all_actions or args.regex):
         return print_error('--exclude-regex argument requires either --all, '
-                           '--all-topics, --topic-types, --all-services or --regex')
+                           '--all-topics, --topic-types, --all-services, --all-actions or --regex')
 
     if args.exclude_topics and not \
             (args.all or args.all_topics or args.topic_types or args.regex):
@@ -257,14 +270,22 @@ def validate_parsed_arguments(args, uri) -> str:
         return print_error('--exclude-services argument requires either --all, --all-services '
                            'or --regex')
 
+    if args.exclude_actions and not (args.all or args.all_actions or args.regex):
+        return print_error('--exclude-actions argument requires either --all, --all-actions '
+                           'or --regex')
+
     if (args.all or args.all_services) and args.services:
         print(print_warn('--all or --all-services will override --services'))
 
     if (args.all or args.all_topics) and args.topics:
         print(print_warn('--all or --all-topics will override --topics'))
 
-    if (args.all or args.all_topics or args.all_services) and args.regex:
-        print(print_warn('--all, --all-topics or --all-services will override --regex'))
+    if (args.all or args.all_actions) and args.actions:
+        print(print_warn('--all or --all-actions will override --actions'))
+
+    if (args.all or args.all_topics or args.all_services or args.all_actions) and args.regex:
+        print(print_warn('--all, --all-topics --all-services or --all-actions will override '
+                         '--regex'))
 
     if os.path.isdir(uri):
         return print_error("Output folder '{}' already exists.".format(uri))
@@ -281,6 +302,8 @@ def validate_parsed_arguments(args, uri) -> str:
     if args.compression_queue_size < 0:
         return print_error('Compression queue size must be at least 0.')
 
+    return ''
+
 
 class RecordVerb(VerbExtension):
     """Record ROS data to a bag."""
@@ -330,11 +353,15 @@ def main(self, *, args):  # noqa: D102
         record_options = RecordOptions()
         record_options.all_topics = args.all_topics or args.all
         record_options.all_services = args.all_services or args.all
+        record_options.all_actions = args.all_actions or args.all
         record_options.is_discovery_disabled = args.no_discovery
         record_options.topics = args.topics
         record_options.topic_types = args.topic_types
         # Convert service name to service event topic name
         record_options.services = convert_service_to_service_event_topic(args.services)
+        # Covert action name to internal topic name and internal service event topic name
+        record_options.actions = convert_action_to_all_related_topics(args.actions)
+
         record_options.exclude_topic_types = args.exclude_topic_types
         record_options.rmw_serialization_format = args.serialization_format
         record_options.topic_polling_interval = datetime.timedelta(
@@ -344,6 +371,7 @@ def main(self, *, args):  # noqa: D102
         record_options.exclude_topics = args.exclude_topics if args.exclude_topics else []
         record_options.exclude_service_events = \
             convert_service_to_service_event_topic(args.exclude_services)
+        record_options.exclude_actions = convert_action_to_all_related_topics(args.exclude_actions)
         record_options.node_prefix = NODE_NAME_PREFIX
         record_options.compression_mode = args.compression_mode
         record_options.compression_format = args.compression_format

ros2bag/test/test_recorder_args_parser.py

@@ -63,6 +63,16 @@ def test_recorder_services_list_argument(test_arguments_parser):
     assert output_path.as_posix() == args.output
 
 
+def test_recorder_actions_list_argument(test_arguments_parser):
+    """Test recorder --actions list argument parser."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--actions', 'action1', 'action2', '--output', output_path.as_posix()]
+    )
+    assert ['action1', 'action2'] == args.actions
+    assert output_path.as_posix() == args.output
+
+
 def test_recorder_services_and_positional_topics_list_arguments(test_arguments_parser):
     """Test recorder --services list and positional topics list arguments parser."""
     output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
@@ -74,6 +84,17 @@ def test_recorder_services_and_positional_topics_list_arguments(test_arguments_p
     assert output_path.as_posix() == args.output
 
 
+def test_recorder_actions_and_positional_topics_list_arguments(test_arguments_parser):
+    """Test recorder --actions list and positional topics list arguments parser."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--output', output_path.as_posix(),
+         '--actions', 'action1', 'action2', '--', 'topic1', 'topic2'])
+    assert ['action1', 'action2'] == args.actions
+    assert ['topic1', 'topic2'] == args.topics_positional
+    assert output_path.as_posix() == args.output
+
+
 def test_recorder_services_and_optional_topics_list_arguments(test_arguments_parser):
     """Test recorder --services list and optional --topics list arguments parser."""
     output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
@@ -85,6 +106,41 @@ def test_recorder_services_and_optional_topics_list_arguments(test_arguments_par
     assert output_path.as_posix() == args.output
 
 
+def test_recorder_actions_and_optional_topics_list_arguments(test_arguments_parser):
+    """Test recorder --actions list and optional --topics list arguments parser."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--output', output_path.as_posix(),
+         '--actions', 'action1', 'action2', '--topics', 'topic1', 'topic2'])
+    assert ['action1', 'action2'] == args.actions
+    assert ['topic1', 'topic2'] == args.topics
+    assert output_path.as_posix() == args.output
+
+
+def test_recorder_services_and_actions_list_arguments(test_arguments_parser):
+    """Test recorder --services list and --actions list arguments parser."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--output', output_path.as_posix(),
+         '--services', 'service1', 'service2', '--actions', 'action1', 'action2'])
+    assert ['service1', 'service2'] == args.services
+    assert ['action1', 'action2'] == args.actions
+    assert output_path.as_posix() == args.output
+
+
+def test_recorder_services_actions_and_topics_list_arguments(test_arguments_parser):
+    """Test recorder --services list, --actions list and --topics list arguments parser."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--output', output_path.as_posix(),
+         '--services', 'service1', 'service2', '--actions', 'action1', 'action2',
+         '--topics', 'topic1', 'topic2'])
+    assert ['service1', 'service2'] == args.services
+    assert ['action1', 'action2'] == args.actions
+    assert ['topic1', 'topic2'] == args.topics
+    assert output_path.as_posix() == args.output
+
+
 def test_recorder_topic_types_list_argument(test_arguments_parser):
     """Test recorder --topic-types list argument parser."""
     output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
@@ -124,6 +180,16 @@ def test_recorder_exclude_services_list_argument(test_arguments_parser):
     assert output_path.as_posix() == args.output
 
 
+def test_recorder_exclude_actions_list_argument(test_arguments_parser):
+    """Test recorder --exclude-actions list argument parser."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--exclude-actions', 'action1', 'action2', '--output', output_path.as_posix()]
+    )
+    assert ['action1', 'action2'] == args.exclude_actions
+    assert output_path.as_posix() == args.output
+
+
 def test_recorder_custom_data_list_argument(test_arguments_parser):
     """Test recorder --custom-data list argument parser."""
     output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
@@ -138,21 +204,22 @@ def test_recorder_validate_exclude_regex_needs_inclusive_args(test_arguments_par
     """Test that --exclude-regex needs inclusive arguments."""
     output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
     args = test_arguments_parser.parse_args(
-        ['--exclude-regex', '%s-%s', '--services', 'service1', '--topics', 'topic1',
-         '--output', output_path.as_posix()]
+        ['--exclude-regex', '%s-%s', '--services', 'service1', '--topics', 'topic1', '--actions',
+         'action1', '--output', output_path.as_posix()]
     )
     assert '%s-%s' == args.exclude_regex
     assert args.all is False
     assert args.all_topics is False
     assert [] == args.topic_types
     assert args.all_services is False
+    assert args.all_actions is False
     assert '' == args.regex
 
     uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S')
     error_str = validate_parsed_arguments(args, uri)
     assert error_str is not None
     expected_output = '--exclude-regex argument requires either --all, ' \
-                      '--all-topics, --topic-types, --all-services or --regex'
+                      '--all-topics, --topic-types, --all-services, --all-actions or --regex'
     matches = expected_output in error_str
     assert matches, ERROR_STRING_MSG.format(expected_output, error_str)
 
@@ -227,3 +294,26 @@ def test_recorder_validate_exclude_services_needs_inclusive_args(test_arguments_
     'or --regex'
     matches = expected_output in error_str
     assert matches, ERROR_STRING_MSG.format(expected_output, error_str)
+
+
+def test_recorder_validate_exclude_actions_needs_inclusive_args(test_arguments_parser):
+    """Test that --exclude-actions needs at least --all, --all-actions or --regex arguments."""
+    output_path = RESOURCES_PATH / 'ros2bag_tmp_file'
+    args = test_arguments_parser.parse_args(
+        ['--exclude-actions', 'action1', '--actions', 'action1', '--all-topics',
+         '--output', output_path.as_posix()]
+    )
+    assert ['action1'] == args.exclude_actions
+    assert args.all is False
+    assert args.all_topics is True
+    assert [] == args.topic_types
+    assert args.all_services is False
+    assert '' == args.regex
+    assert '' == args.exclude_regex
+
+    uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S')
+    error_str = validate_parsed_arguments(args, uri)
+    assert error_str is not None
+    expected_output = '--exclude-actions argument requires either --all, --all-actions or --regex'
+    matches = expected_output in error_str
+    assert matches, ERROR_STRING_MSG.format(expected_output, error_str)

rosbag2_cpp/CMakeLists.txt

@@ -73,7 +73,8 @@ add_library(${PROJECT_NAME} SHARED
   src/rosbag2_cpp/writer.cpp
   src/rosbag2_cpp/writers/sequential_writer.cpp
   src/rosbag2_cpp/reindexer.cpp
-  src/rosbag2_cpp/service_utils.cpp)
+  src/rosbag2_cpp/service_utils.cpp
+  src/rosbag2_cpp/action_utils.cpp)
 
 target_link_libraries(${PROJECT_NAME} PUBLIC
   pluginlib::pluginlib
@@ -303,6 +304,16 @@ if(BUILD_TESTING)
       ${test_msgs_TARGETS}
     )
   endif()
+
+  ament_add_gmock(test_action_utils
+    test/rosbag2_cpp/test_action_utils.cpp)
+  if(TARGET test_action_utils)
+    target_link_libraries(test_action_utils
+      ${PROJECT_NAME}
+      rosbag2_test_common::rosbag2_test_common
+      ${test_msgs_TARGETS}
+    )
+  endif()
 endif()
 
 ament_package()