IMU Filter (#134)

* Add imu_filter launch file and added madgwick entry to filter

* Add imu filter to generator

* Rename imu_filter_node to imu_filter_madgwick

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=[

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()
 

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,

clearpath_sensors/config/imu_filter.yaml

@@ -1,4 +1,4 @@
-imu_filter_node:
+imu_filter_madgwick:
   ros__parameters:
     use_sim_time: False
     stateless: false

clearpath_sensors/launch/imu_filter.launch.py

@@ -0,0 +1,132 @@
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <[email protected]>
+# @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

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