From 86addee3188e47989dea01f0e3eb1360aee81c7c Mon Sep 17 00:00:00 2001
From: Thomas Chou <thomas@wytron.com.tw>
Date: Sat, 27 Apr 2024 17:43:58 -0700
Subject: [PATCH] bringup: add madgwick support

Enable madgwick filter to fuse imu and magnetometer.

Signed-off-by: Thomas Chou <thomas@wytron.com.tw>
---
 README.md                                   | 15 +++++++++++++
 linorobot2_bringup/launch/bringup.launch.py | 25 ++++++++++++++++++++-
 linorobot2_bringup/package.xml              |  1 +
 3 files changed, 40 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 9f231c0e..bcf7b035 100644
--- a/README.md
+++ b/README.md
@@ -172,6 +172,21 @@ Optional parameters:
     ros2 launch linorobot2_bringup bringup.launch.py micro_ros_transport:=udp4 micro_ros_port:=8888
     ```
 
+- **madgwick** - Set to true to enable magnetometer support. The madgwick filter will fuse imu/data_raw and imu/mag to imu/data. You may visualize the IMU and manetometer by [enable the IMU and magetometer plug-ins](https://automaticaddison.com/how-to-publish-imu-data-using-ros-and-the-bno055-imu-sensor/) in RVIZ2. The ekf filter configuration will need update, as only 'vyaw' is enabled in the default configuration. Both IMU and magnetometer must be calibrated, otherwise the robot's pose will rotate.
+
+    ```
+    # enable magnetometer support
+    ros2 launch linorobot2_bringup bringup.launch.py madgwick:=true orientation_stddev:=0.01
+
+    linorobot2_ws/src/linorobot2/linorobot2_base/config/ekf.yaml
+        imu0: imu/data
+        imu0_config: [false, false, false,
+                      false, false, true,
+                      false, false, false,
+                      false, false, true,
+                      true, true, false]
+   ```
+
 - **joy** - Set to true to run the joystick node in the background. (Tested on Logitech F710).
 
 Always wait for the microROS agent to be connected before running any application (ie. creating a map or autonomous navigation). Once connected, the agent will print:
diff --git a/linorobot2_bringup/launch/bringup.launch.py b/linorobot2_bringup/launch/bringup.launch.py
index 23d405fe..007630e5 100644
--- a/linorobot2_bringup/launch/bringup.launch.py
+++ b/linorobot2_bringup/launch/bringup.launch.py
@@ -86,13 +86,36 @@ def generate_launch_description():
             default_value='/odom',
             description='EKF out odometry topic'
         ),
-        
+
+        DeclareLaunchArgument(
+            name='madgwick',
+            default_value='false',
+            description='Use madgwick to fuse imu and magnetometer'
+        ),
+
+        DeclareLaunchArgument(
+            name='orientation_stddev',
+            default_value='0.003162278',
+            description='Madgwick orientation stddev'
+        ),
+
         DeclareLaunchArgument(
             name='joy', 
             default_value='false',
             description='Use Joystick'
         ),
 
+        Node(
+            condition=IfCondition(LaunchConfiguration("madgwick")),
+            package='imu_filter_madgwick',
+            executable='imu_filter_madgwick_node',
+            name='madgwick_filter_node',
+            output='screen',
+            parameters=[
+                {'orientation_stddev' : LaunchConfiguration('orientation_stddev')}
+            ]
+        ),
+
         Node(
             package='robot_localization',
             executable='ekf_node',
diff --git a/linorobot2_bringup/package.xml b/linorobot2_bringup/package.xml
index 3fdd3ee3..7ec88c35 100644
--- a/linorobot2_bringup/package.xml
+++ b/linorobot2_bringup/package.xml
@@ -10,6 +10,7 @@
   <url type="repository">https://github.com/linorobot/linorobot2</url>
   <url type="bugtracker">https://github.com/linorobot/linorobot2/issues</url>
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>imu_tools</exec_depend>
   <exec_depend>robot_localization</exec_depend>
   <exec_depend>joy_linux</exec_depend>
   <exec_depend>teleop_twist_joy</exec_depend>