Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

IMU Filter #134

Merged
merged 7 commits into from
Jan 28, 2025
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -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=[
Original file line number Diff line number Diff line change
@@ -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()

Original file line number Diff line number Diff line change
@@ -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,
2 changes: 1 addition & 1 deletion clearpath_sensors/config/imu_filter.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
imu_filter_node:
imu_filter_madgwick:
ros__parameters:
use_sim_time: False
stateless: false
132 changes: 132 additions & 0 deletions clearpath_sensors/launch/imu_filter.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,132 @@
# Software License Agreement (BSD)
#
# @author Luis Camero <lcamero@clearpathrobotics.com>
# @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
6 changes: 3 additions & 3 deletions clearpath_sensors/launch/phidgets_spatial.launch.py
Original file line number Diff line number Diff line change
@@ -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