Add bag recorder config and launch file to diagnostics

luis-camero · luis-camero · commit 649c62379969 · 2025-05-15T14:50:39.000-04:00
diff --git a/clearpath_diagnostics/config/recorder.yaml b/clearpath_diagnostics/config/recorder.yaml
@@ -0,0 +1,50 @@
+recorder:
+  ros__parameters:
+
+    use_sim_time: false
+
+    record:
+      # all_topics: false
+      # all_services: false
+      # all_actions: false
+      # is_discovery_disabled: true
+      #topics: ["topic", "other_topic"]
+      #topic_types: ["std_msgs/msg/Header", "geometry_msgs/msg/Pose"]
+      #exclude_topic_types: ["sensor_msgs/msg/Image"]
+      #services: ["service", "other_service"]
+      #actions: ["action", "other_action"]
+      #rmw_serialization_format: "cdr"
+      #topic_polling_interval:
+      #  sec: 0
+      #  nsec: 10000000
+      regex: "(.*)platform|wiferion|robot_description|tf|cmd_vel|imu(.*)"
+      # add wiferion, tf, tf_static, robot_description, "(.*)platform|wiferion(.*)", imu_data, all cmd_vel
+      # exclude_regex: "(.*)"
+      # exclude_topics: ["exclude_topic", "other_exclude_topic"]
+      # exclude_services: ["exclude_service", "other_exclude_service"]
+      # exclude_actions: ["exclude_action", "other_exclude_action"]
+      # node_prefix: "prefix"
+      # compression_mode: "file"
+      # compression_format: "zstd"
+      # compression_queue_size: 10
+      # compression_threads: 2
+      # compression_threads_priority: -1
+      # qos_profile_overrides_path: ""
+      include_hidden_topics: true
+      # include_unpublished_topics: true
+      # ignore_leaf_topics: false
+      # start_paused: false
+      # disable_keyboard_controls: true
+
+    storage:
+      uri: /etc/clearpath/bags
+      # storage_id: "sqlite3"
+      # storage_config_uri: ""
+      # max_bagfile_size: 2147483646
+      max_bagfile_duration: 600
+      # max_cache_size: 16777216
+      storage_preset_profile: "zstd_fast"
+      # snapshot_mode: true
+      # custom_data: ["key1=value1", "key2=value2"]
+      # start_time_ns: 0
+      # end_time_ns: 100000
diff --git a/clearpath_diagnostics/launch/recorder.launch.py b/clearpath_diagnostics/launch/recorder.launch.py
@@ -0,0 +1,143 @@
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <lcamero@clearpathrobotics.com>
+# @copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+import datetime
+import os
+import yaml
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    # Launch Configurations
+    namespace = str(
+        LaunchConfiguration('namespace').perform(context))
+    parameters = str(
+        LaunchConfiguration('parameters').perform(context))
+    enable_recorder = bool(
+        LaunchConfiguration('enable_recorder').perform(context) == 'true')
+
+    # Extract Parameters
+    content = yaml.safe_load(open(parameters))
+    params = {}
+    for i in content:
+        if 'ros__parameters' in content[i]:
+            params = content[i]['ros__parameters']
+    found = False
+    while not found:
+        if 'ros__parameters' in content:
+            params = content['ros__parameters']
+            found = True
+        else:
+            content = content.pop(list(content)[0])
+        if not isinstance(content, dict):
+            break
+    if not found:
+        if enable_recorder:
+            return [
+                Node(
+                    package='rosbag2_transport',
+                    executable='recorder',
+                    name='recorder',
+                    output="screen",
+                    namespace=namespace,
+                    parameters=[parameters],
+                )
+            ]
+        else:
+            return []
+
+    # Create Timestamp Directory
+    flat = False
+    path = None
+    if 'storage' in params:
+        if 'uri' in params['storage']:
+            path = params['storage']['uri']
+    elif 'storage.uri' in params:
+        path = params['storage.uri']
+        flat = True
+    if not path:
+        path = '/etc/clearpath/bags'
+    if not os.path.exists(path):
+        os.makedirs(path)
+    timestamp_path = os.path.join(
+        path, 'rosbag2_' + datetime.datetime.now().strftime('%Y_%m_%d-%H_%M_%S'))
+    if flat:
+        params['storage.uri'] = timestamp_path
+    else:
+        params['storage']['uri'] = timestamp_path
+
+    # Node
+    node = Node(
+        package='rosbag2_transport',
+        executable='recorder',
+        name='recorder',
+        output="screen",
+        namespace=namespace,
+        parameters=[params],
+    )
+
+    nodes = []
+    if enable_recorder:
+        nodes.append(node)
+    return nodes
+
+
+def generate_launch_description():
+    arg_namespace = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Robot namespace'
+    )
+
+    arg_parameters = DeclareLaunchArgument(
+        'parameters',
+        default_value=PathJoinSubstitution([
+          FindPackageShare('clearpath_diagnostics'),
+          'config',
+          'recorder.yaml'
+        ]),
+        description='Recorder node parameters'
+    )
+
+    arg_enable_recorder = DeclareLaunchArgument(
+        'enable_recorder',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Enable recording'
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(arg_namespace)
+    ld.add_action(arg_parameters)
+    ld.add_action(arg_enable_recorder)
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld
