diff --git a/clearpath_diagnostics/config/recorder.yaml b/clearpath_diagnostics/config/recorder.yaml new file mode 100644 index 00000000..f3e63209 --- /dev/null +++ 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 new file mode 100644 index 00000000..c08e689c --- /dev/null +++ b/clearpath_diagnostics/launch/recorder.launch.py @@ -0,0 +1,144 @@ +# Software License Agreement (BSD) +# +# @author Luis Camero +# @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 + +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 +import yaml + + +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 diff --git a/clearpath_generator_common/clearpath_generator_common/param/platform.py b/clearpath_generator_common/clearpath_generator_common/param/platform.py index 6d4bc664..f8f51353 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/platform.py +++ b/clearpath_generator_common/clearpath_generator_common/param/platform.py @@ -56,6 +56,7 @@ class PlatformParam(): TELEOP_INTERACTIVE_MARKERS = 'teleop_interactive_markers' TELEOP_JOY = 'teleop_joy' TWIST_MUX = 'twist_mux' + RECORDER = 'recorder' NOT_APPLICABLE = 'not_applicable' @@ -67,7 +68,8 @@ class PlatformParam(): LOCALIZATION, TELEOP_INTERACTIVE_MARKERS, TELEOP_JOY, - TWIST_MUX + TWIST_MUX, + RECORDER ] class BaseParam(): @@ -508,6 +510,15 @@ def __init__(self, super().__init__(parameter, clearpath_config, param_path) self.default_parameter_file_path = 'config' + class RecorderParam(BaseParam): + def __init__(self, + parameter: str, + clearpath_config: ClearpathConfig, + param_path: str): + super().__init__(parameter, clearpath_config, param_path) + self.default_parameter_file_path = 'config' + self.default_parameter_file_package = Package(self.CLEARPATH_DIAGNOSTICS) + PARAMETER = { IMU_FILTER: ImuFilterParam, DIAGNOSTIC_AGGREGATOR: DiagnosticsAggregatorParam, @@ -515,6 +526,7 @@ def __init__(self, LOCALIZATION: LocalizationParam, TELEOP_JOY: TeleopJoyParam, TWIST_MUX: TwistMuxParam, + RECORDER: RecorderParam, } def __new__(cls,