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_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index d7059d1f..d3a38dd0 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -345,6 +345,23 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: namespace=self.namespace, ) + # ROS Bag Recorder + self.recorder_params = LaunchFile.LaunchArg( + 'recorder_params', + default_value=os.path.join(self.platform_params_path, 'recorder.yaml'), + ) + + self.recorder_args = [ + ('parameters', LaunchFile.Variable('recorder_params')), + ('namespace', self.namespace), + ('enable_recorder', str(self.clearpath_config.platform.enable_recorder).lower()) + ] + + self.recorder_launch = LaunchFile( + 'recorder', + package=clearpath_diagnostics_package, + args=self.recorder_args) + # Components required for each platform common_platform_components = [ self.wireless_watcher_node, @@ -352,6 +369,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: self.diag_aggregator_params, self.diagnostics_launch, self.battery_state_control, + self.recorder_params, + self.recorder_launch, ] # Only add estimator when no BMS is present