Pr mag

README.md

@@ -157,6 +157,13 @@ Optional parameters:
     ```
     ros2 launch linorobot2_bringup bringup.launch.py base_serial_port:=/dev/ttyACM1
     ```
+
+- **micro_ros_baudrate** - micro-ROS serial baudrate. default 115200.
+
+    ```
+    ros2 launch linorobot2_bringup bringup.launch.py base_serial_port:=/dev/ttyUSB0 micro_ros_baudrate:=921600
+    ```
+
 - **micro_ros_transport** - micro-ROS transport. default serial.
 - **micro_ros_port** - micro-ROS udp/tcp port number. default 8888.
 
@@ -165,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:

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',

linorobot2_bringup/launch/default_robot.launch.py

@@ -37,6 +37,12 @@ def generate_launch_description():
             description='Linorobot Base Serial Port'
         ),
 
+        DeclareLaunchArgument(
+            name='micro_ros_baudrate', 
+            default_value='115200',
+            description='micro-ROS baudrate'
+        ),
+
         DeclareLaunchArgument(
             name='micro_ros_transport',
             default_value='serial',
@@ -55,7 +61,7 @@ def generate_launch_description():
             executable='micro_ros_agent',
             name='micro_ros_agent',
             output='screen',
-            arguments=['serial', '--dev', LaunchConfiguration("base_serial_port")]
+            arguments=['serial', '--dev', LaunchConfiguration("base_serial_port"), '--baudrate', LaunchConfiguration("micro_ros_baudrate")]
         ),
 
         Node(

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>

linorobot2_navigation/config/navigation.yaml

@@ -396,3 +396,17 @@ waypoint_follower:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: true
       waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.0, 2.5]
+    min_velocity: [-0.5, 0.0, -2.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

linorobot2_navigation/config/navigation_sim.yaml

@@ -396,3 +396,17 @@ waypoint_follower:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: true
       waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.0, 2.5]
+    min_velocity: [-0.5, 0.0, -2.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0