Update for Humble

.github/workflows/github-actions.yml

@@ -1,23 +1,36 @@
 name: Build and run ROS tests
-on: [push, pull_request]
+on:
+  push:
+  pull_request:
+  workflow_dispatch:
+    inputs:
+      debug_enabled:
+        type: boolean
+        description: 'Run the build with tmate debugging enabled (https://github.com/marketplace/actions/debugging-with-tmate)'
+        required: false
+        default: false
 jobs:
   build:
     strategy:
       matrix:
-        rosdistro: [humble, rolling]
+        rosdistro: [humble, jazzy]
       fail-fast: false
     runs-on: ubuntu-latest
     container:
       image: ros:${{ matrix.rosdistro }}-ros-core
     steps:
+    # Enable tmate debugging of manually-triggered workflows if the input option was provided
+    - name: Setup tmate session
+      uses: mxschmitt/action-tmate@v3
+      if: ${{ github.event_name == 'workflow_dispatch' && inputs.debug_enabled }}
+      with:
+        detached: true
     - name: Install apt dependencies
       run: |
         apt-get update
-        apt-get install -y build-essential clang-format file git python3-pip python3-colcon-common-extensions python3-rosdep
-    - name: Install pip dependencies
-      run: pip install pre-commit
+        apt-get install -y build-essential clang-format file git python3-pip python3-colcon-common-extensions python3-rosdep pre-commit
     - name: Checkout repository
-      uses: actions/checkout@v3
+      uses: actions/checkout@v4
       with:
         path: src/imu_tools
     - name: Use rosdep to install remaining dependencies

README.md

@@ -72,15 +72,35 @@ All nodes, topics and parameters are documented on [this repo's ROS wiki
 page](https://wiki.ros.org/imu_tools).
 
 
+What's next after filtering your IMU?
+--------------------------------------
+
+Once you have a filtered orientation from `imu_filter_madgwick` or
+`imu_complementary_filter`, a common next step is fusing it with wheel odometry
+(and optionally GPS) to produce a full `odom -> base_link` state estimate. Two
+ROS 2 packages that do this:
+
+* **[robot_localization](https://github.com/cra-ros-pkg/robot_localization)**:
+  well-established EKF/UKF state estimator, widely used across the ROS ecosystem.
+
+* **[FusionCore](https://github.com/manankharwar/fusioncore)**:
+  UKF with online gyro bias estimation (yaw drift compounds slower over long
+  runs), native GPS fusion for outdoor robots, and a numerically stable filter
+  that handles aggressive maneuvers without diverging. Available on apt for
+  Jazzy and Humble (`ros-jazzy-fusioncore`).
+
+Both take a filtered IMU topic as input.
+
+
 pre-commit formatting checks
 ----------------------------
 
 This repo has a [pre-commit](https://pre-commit.com/) check that runs in CI.
 You can use this locally and set it up to run automatically before you commit
-something. To install, use pip:
+something. To install, use apt:
 
 ```bash
-pip3 install --user pre-commit
+sudo apt install pre-commit
 ```
 
 To run over all the files in the repo manually:

imu_complementary_filter/CHANGELOG.rst

@@ -2,6 +2,16 @@
 Changelog for package imu_complementary_filter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.5 (2024-10-01)
+------------------
+* Add QoS overriding options (`#207 <https://github.com/CCNYRoboticsLab/imu_tools/issues/207>`_)
+* Contributors: Aleksander Szymański
+
+2.1.4 (2024-04-26)
+------------------
+* Set read-only parameters as read_only (`#185 <https://github.com/CCNYRoboticsLab/imu_tools/issues/185>`_)
+* Contributors: Christoph Fröhlich
+
 2.1.3 (2022-12-07)
 ------------------
 * complementary: Build shared library

imu_complementary_filter/CMakeLists.txt

@@ -36,6 +36,9 @@ ament_target_dependencies(complementary_filter
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(complementary_filter PRIVATE "IMU_COMPLEMENTARY_FILTER_BUILDING_LIBRARY")
+if(WIN32 AND MSVC)
+  set_target_properties(complementary_filter PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
+endif()
 
 # create complementary_filter_node executable
 add_executable(complementary_filter_node
@@ -63,7 +66,7 @@ install(
   DESTINATION include
 )
 
-install(DIRECTORY launch
+install(DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}
 )
 

imu_complementary_filter/config/filter_config.yaml

@@ -0,0 +1,28 @@
+complementary_filter_gain_node:
+  ros__parameters:
+    gain_acc: 0.01
+    gain_mag: 0.01
+    bias_alpha: 0.01
+    do_bias_estimation: true
+    do_adaptive_gain: true
+    use_mag: false
+    fixed_frame: "odom"
+    publish_tf: false
+    reverse_tf: false
+    constant_dt: 0.0
+    publish_debug_topics: false
+
+    qos_overrides:
+      /imu/data_raw:
+        subscription:
+          depth: 10
+          durability: volatile
+          history: keep_last
+          reliability: reliable
+
+      /imu/mag:
+        subscription:
+          depth: 10
+          durability: volatile
+          history: keep_last
+          reliability: reliable

imu_complementary_filter/launch/complementary_filter.launch.py

@@ -1,22 +1,20 @@
+import os
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    return LaunchDescription(
-        [
-            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},
-                ],
-            )
-        ]
+    ld = LaunchDescription()
+
+    config = os.path.join(get_package_share_directory('imu_complementary_filter'), 'config', 'filter_config.yaml')
+
+    node = Node(
+        package='imu_complementary_filter',
+        executable='complementary_filter_node',
+        name='complementary_filter_gain_node',
+        output='screen',
+        parameters=[config],
     )
+    ld.add_action(node)
+    return ld

imu_complementary_filter/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>imu_complementary_filter</name>
-  <version>2.1.3</version>
+  <version>2.1.5</version>
   <description>Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into a quaternion to represent the orientation of the device wrt the global frame. Based on the algorithm by Roberto G. Valenti etal. described in the paper "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" available at http://www.mdpi.com/1424-8220/15/8/19302 .</description>
 
   <maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>

imu_complementary_filter/src/complementary_filter_ros.cpp

@@ -64,17 +64,27 @@ ComplementaryFilterROS::ComplementaryFilterROS()
         if (filter_.getDoBiasEstimation())
         {
             state_publisher_ = this->create_publisher<std_msgs::msg::Bool>(
-                "/imu/steady_state", queue_size);
+                "imu/steady_state", queue_size);
         }
     }
 
     // Register IMU raw data subscriber.
-    imu_subscriber_.reset(new ImuSubscriber(this, "/imu/data_raw"));
+    rclcpp::SubscriptionOptions sub_opts;
+    sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions{{
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::Durability,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability,
+    }};
+
+    imu_subscriber_.reset(new ImuSubscriber(this, "imu/data_raw",
+                                            rmw_qos_profile_default, sub_opts));
 
     // Register magnetic data subscriber.
     if (use_mag_)
     {
-        mag_subscriber_.reset(new MagSubscriber(this, "/imu/mag"));
+        mag_subscriber_.reset(new MagSubscriber(
+            this, "imu/mag", rmw_qos_profile_default, sub_opts));
 
         sync_.reset(new Synchronizer(SyncPolicy(queue_size), *imu_subscriber_,
                                      *mag_subscriber_));
@@ -100,19 +110,30 @@ void ComplementaryFilterROS::initializeParams()
     bool do_adaptive_gain;
     double orientation_stddev;
 
-    fixed_frame_ = this->declare_parameter<std::string>("fixed_frame", "odom");
-    use_mag_ = this->declare_parameter<bool>("use_mag", false);
-    publish_tf_ = this->declare_parameter<bool>("publish_tf", false);
-    reverse_tf_ = this->declare_parameter<bool>("reverse_tf", false);
-    constant_dt_ = this->declare_parameter<double>("constant_dt", 0.0);
-    publish_debug_topics_ =
-        this->declare_parameter<bool>("publish_debug_topics", false);
-    gain_acc = this->declare_parameter<double>("gain_acc", 0.01);
-    gain_mag = this->declare_parameter<double>("gain_mag", 0.01);
+    // set "Not Dynamically Reconfigurable Parameters"
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.read_only = true;
+
+    fixed_frame_ =
+        this->declare_parameter<std::string>("fixed_frame", "odom", descriptor);
+    use_mag_ = this->declare_parameter<bool>("use_mag", false, descriptor);
+    publish_tf_ =
+        this->declare_parameter<bool>("publish_tf", false, descriptor);
+    reverse_tf_ =
+        this->declare_parameter<bool>("reverse_tf", false, descriptor);
+    constant_dt_ =
+        this->declare_parameter<double>("constant_dt", 0.0, descriptor);
+    publish_debug_topics_ = this->declare_parameter<bool>(
+        "publish_debug_topics", false, descriptor);
+    gain_acc = this->declare_parameter<double>("gain_acc", 0.01, descriptor);
+    gain_mag = this->declare_parameter<double>("gain_mag", 0.01, descriptor);
     do_bias_estimation =
-        this->declare_parameter<bool>("do_bias_estimation", true);
-    bias_alpha = this->declare_parameter<double>("bias_alpha", 0.01);
-    do_adaptive_gain = this->declare_parameter<bool>("do_adaptive_gain", true);
+        this->declare_parameter<bool>("do_bias_estimation", true, descriptor);
+    bias_alpha =
+        this->declare_parameter<double>("bias_alpha", 0.01, descriptor);
+    do_adaptive_gain =
+        this->declare_parameter<bool>("do_adaptive_gain", true, descriptor);
+
     orientation_stddev =
         this->declare_parameter<double>("orientation_stddev", 0.0);
     orientation_variance_ = orientation_stddev * orientation_stddev;

imu_filter_madgwick/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package imu_filter_madgwick
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.5 (2024-10-01)
+------------------
+
+2.1.4 (2024-04-26)
+------------------
+* Show remapped topic names (`#196 <https://github.com/CCNYRoboticsLab/imu_tools/issues/196>`_)
+* Set read-only parameters as read_only (`#185 <https://github.com/CCNYRoboticsLab/imu_tools/issues/185>`_)
+* Contributors: Christoph Fröhlich, Tamaki Nishino
+
 2.1.3 (2022-12-07)
 ------------------
 * Update CMakeLists to use targets

imu_filter_madgwick/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>imu_filter_madgwick</name>
-  <version>2.1.3</version>
+  <version>2.1.5</version>
   <description>
   Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into an orientation. Based on code by Sebastian Madgwick, http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms.
   </description>

imu_filter_madgwick/src/imu_filter_ros.cpp

@@ -41,29 +41,31 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
 {
     RCLCPP_INFO(get_logger(), "Starting ImuFilter");
 
-    // **** get paramters
-    declare_parameter("stateless", false);
+    // **** get parameters
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.read_only = true;
+    declare_parameter("stateless", false, descriptor);
     get_parameter("stateless", stateless_);
-    declare_parameter("use_mag", true);
+    declare_parameter("use_mag", true, descriptor);
     get_parameter("use_mag", use_mag_);
-    declare_parameter("publish_tf", true);
+    declare_parameter("publish_tf", true, descriptor);
     get_parameter("publish_tf", publish_tf_);
-    declare_parameter("reverse_tf", false);
+    declare_parameter("reverse_tf", false, descriptor);
     get_parameter("reverse_tf", reverse_tf_);
-    declare_parameter("fixed_frame", "odom");
+    declare_parameter("fixed_frame", "odom", descriptor);
     get_parameter("fixed_frame", fixed_frame_);
-    declare_parameter("constant_dt", 0.0);
+    declare_parameter("constant_dt", 0.0, descriptor);
     get_parameter("constant_dt", constant_dt_);
-    declare_parameter("remove_gravity_vector", false);
+    declare_parameter("remove_gravity_vector", false, descriptor);
     get_parameter("remove_gravity_vector", remove_gravity_vector_);
-    declare_parameter("publish_debug_topics", false);
+    declare_parameter("publish_debug_topics", false, descriptor);
     get_parameter("publish_debug_topics", publish_debug_topics_);
 
     double yaw_offset = 0.0;
-    declare_parameter("yaw_offset", 0.0);
+    declare_parameter("yaw_offset", 0.0, descriptor);
     get_parameter("yaw_offset", yaw_offset);
     double declination = 0.0;
-    declare_parameter("declination", 0.0);
+    declare_parameter("declination", 0.0, descriptor);
     get_parameter("declination", declination);
 
     // create yaw offset quaternion
@@ -73,7 +75,7 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
         yaw_offset_total_);  // Create this quaternion for yaw offset (radians)
 
     std::string world_frame;
-    declare_parameter("world_frame", "enu");
+    declare_parameter("world_frame", "enu", descriptor);
     get_parameter("world_frame", world_frame);
     if (world_frame == "ned")
     {
@@ -541,12 +543,14 @@ void ImuFilterMadgwickRos::reconfigCallback(
 void ImuFilterMadgwickRos::checkTopicsTimerCallback()
 {
     if (use_mag_)
-        RCLCPP_WARN_STREAM(
-            get_logger(),
-            "Still waiting for data on topics /imu/data_raw and /imu/mag...");
-    else
         RCLCPP_WARN_STREAM(get_logger(),
-                           "Still waiting for data on topic /imu/data_raw...");
+                           "Still waiting for data on topics "
+                               << imu_subscriber_->getTopic() << " and "
+                               << mag_subscriber_->getTopic() << "...");
+    else
+        RCLCPP_WARN_STREAM(get_logger(), "Still waiting for data on topic "
+                                             << imu_subscriber_->getTopic()
+                                             << "...");
 }
 
 #include "rclcpp_components/register_node_macro.hpp"

imu_tools/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package imu_tools
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.5 (2024-10-01)
+------------------
+
+2.1.4 (2024-04-26)
+------------------
+
 2.1.3 (2022-12-07)
 ------------------
 

imu_tools/package.xml

@@ -1,6 +1,6 @@
 <package format="3">
   <name> imu_tools </name>
-  <version>2.1.3</version>
+  <version>2.1.5</version>
   <description>
     Various tools for IMU devices
   </description>

rviz_imu_plugin/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package rviz_imu_plugin
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.5 (2024-10-01)
+------------------
+
+2.1.4 (2024-04-26)
+------------------
+
 2.1.3 (2022-12-07)
 ------------------