add imu_filter for fusion in robot_localization

luxonis · Oct 15, 2023 · 3c654bd · 3c654bd
1 parent 8e4a8a1
commit 3c654bd
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 9 deletions.
diff --git a/rae_hw/config/ekf.yaml b/rae_hw/config/ekf.yaml
@@ -85,10 +85,10 @@ ekf_filter_node:
         # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
         # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
         # if unspecified, effectively making this parameter required for each sensor.
-        odom0_config: [true, true, true, # X , Y , Z
-                      false, false, true, # roll , pitch ,yaw
-                      true, true, false, # dX , dY , dZ
-                      false, false, true, # droll , dpitch ,dyaw
+        odom0_config: [true, true, false, # X , Y , Z
+                      false, false, false, # roll , pitch ,yaw
+                      false, false, false, # dX , dY , dZ
+                      false, false, false, # droll , dpitch ,dyaw
                       false, false, false] # ddX , ddY , ddZ
 
         # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
@@ -129,12 +129,12 @@ ekf_filter_node:
 
         # Further input parameter examples
 
-        imu0: /rae/imu/data
+        imu0: imu/data
         imu0_config: [false, false, false,
                       true,  true,  true,
                       false, false, false,
-                      true,  true,  true,
-                      true,  true,  true]
+                      false,  false,  false,
+                      false,  false,  false]
         imu0_differential: false
         imu0_relative: true
         imu0_queue_size: 5

diff --git a/rae_hw/launch/control.launch.py b/rae_hw/launch/control.launch.py
@@ -20,7 +20,7 @@ def launch_setup(context, *args, **kwargs):
         'rae_hw'), 'config', 'controller.yaml')
     run_container = LaunchConfiguration('run_container', default='true')
     enable_battery_status = LaunchConfiguration('enable_battery_status', default='true')
-    enable_localization = LaunchConfiguration('enable_localization', default='false')
+    enable_localization = LaunchConfiguration('enable_localization', default='true')
     return [
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
@@ -36,6 +36,22 @@ def launch_setup(context, *args, **kwargs):
             parameters=[os.path.join(get_package_share_directory(
                 'rae_hw'), 'config', 'ekf.yaml')],
         ),
+        Node(
+                package='imu_complementary_filter',
+                executable='complementary_filter_node',
+                name='complementary_filter_gain_node',
+                output='screen',
+                parameters=[
+                    {'do_bias_estimation': True},
+                    {'do_adaptive_gain': True},
+                    {'use_mag': False},
+                    {'gain_acc': 0.01},
+                    {'gain_mag': 0.01},
+                ],
+                remappings=[
+                    ('/imu/data_raw', '/rae/imu/data'),
+              ]
+        ),
         Node(
             package='controller_manager',
             executable='ros2_control_node',
@@ -73,7 +89,7 @@ def generate_launch_description():
         DeclareLaunchArgument('name', default_value='rae'),
         DeclareLaunchArgument('run_container', default_value='true'),
         DeclareLaunchArgument('enable_battery_status', default_value='true'),
-        DeclareLaunchArgument('enable_localization', default_value='false')
+        DeclareLaunchArgument('enable_localization', default_value='true')
     ]
 
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/rae_hw/package.xml b/rae_hw/package.xml
@@ -27,6 +27,7 @@
   <depend>cv_bridge</depend>
   <depend>audio_msgs</depend>
   <depend>robot_localization</depend>
+  <depend>imu_tools</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>