diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index b635a09..577324a 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -50,7 +50,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: self.imu_0_filter_node = LaunchFile.Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', - name='imu_filter_node', + name='imu_filter_madgwick', namespace=self.namespace, parameters=[LaunchFile.Variable('imu_filter')], remappings=[ diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py index 0f5c2e9..21f94cd 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py @@ -30,6 +30,7 @@ # modification, is not permitted without the express permission # of Clearpath Robotics. from clearpath_config.sensors.types.cameras import BaseCamera +from clearpath_config.sensors.types.imu import BaseIMU, IMUFilter from clearpath_config.sensors.types.sensor import BaseSensor from clearpath_generator_common.common import LaunchFile, Package, ParamFile @@ -50,6 +51,11 @@ class SensorLaunch(): INPUT = 'input_ns' OUTPUT = 'output_ns' CONTAINER = 'container' + # IMU filter + FILTER = 'filter' + INPUT_RAW = 'input_raw' + INPUT_MAG = 'input_mag' + OUTPUT_IMU = 'output' def __init__( self, @@ -97,6 +103,22 @@ def generate(self): (self.CONTAINER, 'image_processing_container') ] )) + # IMU Filter + if self.sensor.get_sensor_type() == BaseIMU.get_sensor_type(): + if self.sensor.filter.TYPE != IMUFilter.NoFilter.TYPE: + sensor_writer.add(LaunchFile( + 'imu_filter', + package=self.CLEARPATH_SENSORS_PACKAGE, + args=[ + (self.NAMESPACE, self.namespace), + (self.PARAMETERS, self.parameters.full_path), + (self.CONTAINER, 'imu_filter_container'), + (self.FILTER, self.sensor.filter.TYPE), + (self.INPUT_RAW, self.sensor.filter.input_raw), + (self.INPUT_MAG, self.sensor.filter.input_mag), + (self.OUTPUT_IMU, self.sensor.filter.output), + ] + )) # Generate sensor launch file sensor_writer.generate_file() diff --git a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py index 206750d..e1c3d6f 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py +++ b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py @@ -32,6 +32,7 @@ from clearpath_config.common.utils.dictionary import merge_dict from clearpath_config.sensors.types.cameras import BaseCamera +from clearpath_config.sensors.types.imu import BaseIMU, IMUFilter from clearpath_config.sensors.types.sensor import BaseSensor from clearpath_generator_common.common import Package, ParamFile @@ -83,6 +84,18 @@ def __init__( default_parameters = merge_dict(default_parameters, republisher_file.parameters) + # IMU Filter + if self.sensor.get_sensor_type() == BaseIMU.get_sensor_type(): + if self.sensor.filter.TYPE != IMUFilter.NoFilter.TYPE: + name = 'imu_filter' + imu_filter_file = ParamFile( + name=name, + package=self.clearpath_sensors_package, + parameters={} + ) + imu_filter_file.read() + default_parameters = merge_dict(default_parameters, imu_filter_file.parameters) + # Parameter file to generate self.param_file = ParamFile( name=self.sensor.name, diff --git a/clearpath_sensors/config/imu_filter.yaml b/clearpath_sensors/config/imu_filter.yaml index c2aed31..34fbed6 100644 --- a/clearpath_sensors/config/imu_filter.yaml +++ b/clearpath_sensors/config/imu_filter.yaml @@ -1,4 +1,4 @@ -imu_filter_node: +imu_filter_madgwick: ros__parameters: use_sim_time: False stateless: false diff --git a/clearpath_sensors/launch/imu_filter.launch.py b/clearpath_sensors/launch/imu_filter.launch.py new file mode 100644 index 0000000..02b7a7e --- /dev/null +++ b/clearpath_sensors/launch/imu_filter.launch.py @@ -0,0 +1,132 @@ +# Software License Agreement (BSD) +# +# @author Luis Camero +# @copyright (c) 2024, 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. +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression + +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + parameters = LaunchConfiguration('parameters') + namespace = LaunchConfiguration('namespace') + container = LaunchConfiguration('container') + input_mag = LaunchConfiguration('input_mag') + input_raw = LaunchConfiguration('input_raw') + output = LaunchConfiguration('output') + + arg_parameters = DeclareLaunchArgument( + 'parameters', + default_value=PathJoinSubstitution([ + FindPackageShare('clearpath_sensors'), + 'config', + 'imu_filter.yaml' + ]) + ) + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='sensors/imu_0' + ) + + arg_container = DeclareLaunchArgument( + 'container', + default_value='' + ) + + arg_filter = DeclareLaunchArgument( + 'filter', + choices=['none', 'madgwick'], + default_value='none' + ) + + arg_input_mag = DeclareLaunchArgument( + 'input_mag', + default_value='mag', + ) + + arg_input_raw = DeclareLaunchArgument( + 'input_raw', + default_value='data_raw' + ) + + arg_output = DeclareLaunchArgument( + 'output', + default_value='data' + ) + + # Filters + composable_nodes = [ + ComposableNode( + package='imu_filter_madgwick', + plugin='ImuFilterMadgwickRos', + name='imu_filter_madgwick', + namespace=namespace, + parameters=[parameters], + condition=LaunchConfigurationEquals('filter', 'madgwick'), + remappings=[ + ('imu/data', output), + ('imu/data_raw', input_raw), + ('imu/mag', input_mag), + ('/tf', 'tf') + ] + ) + ] + + # Create container + imu_filter_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals('container', ''), + name='imu_filter_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen' + ) + + # Use container launched by IMU + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals('container', ''), + composable_node_descriptions=composable_nodes, + target_container=PythonExpression(["'", namespace, '/', container, "'"]) + ) + + ld = LaunchDescription() + ld.add_action(arg_parameters) + ld.add_action(arg_namespace) + ld.add_action(arg_container) + ld.add_action(arg_filter) + ld.add_action(arg_input_mag) + ld.add_action(arg_input_raw) + ld.add_action(arg_output) + ld.add_action(imu_filter_container) + ld.add_action(load_composable_nodes) + return ld diff --git a/clearpath_sensors/launch/phidgets_spatial.launch.py b/clearpath_sensors/launch/phidgets_spatial.launch.py index 64fc657..e3d295d 100644 --- a/clearpath_sensors/launch/phidgets_spatial.launch.py +++ b/clearpath_sensors/launch/phidgets_spatial.launch.py @@ -63,8 +63,8 @@ def generate_launch_description(): ] ) - imu_processing_container = ComposableNodeContainer( - name='imu_processing_container', + imu_filter_container = ComposableNodeContainer( + name='imu_filter_container', namespace=namespace, package='rclcpp_components', executable='component_container', @@ -77,5 +77,5 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(arg_namespace) ld.add_action(arg_parameters) - ld.add_action(imu_processing_container) + ld.add_action(imu_filter_container) return ld