From c8a27cda72ad4e89245e851c732888a089e86065 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 14 Nov 2023 14:08:53 +0100 Subject: [PATCH 01/17] Set read-only parameters as read_only (#185) --- .../src/complementary_filter_ros.cpp | 35 ++++++++++++------- imu_filter_madgwick/src/imu_filter_ros.cpp | 26 +++++++------- 2 files changed, 37 insertions(+), 24 deletions(-) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index e8a9dd74..96f1bdaa 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -100,19 +100,30 @@ void ComplementaryFilterROS::initializeParams() bool do_adaptive_gain; double orientation_stddev; - fixed_frame_ = this->declare_parameter("fixed_frame", "odom"); - use_mag_ = this->declare_parameter("use_mag", false); - publish_tf_ = this->declare_parameter("publish_tf", false); - reverse_tf_ = this->declare_parameter("reverse_tf", false); - constant_dt_ = this->declare_parameter("constant_dt", 0.0); - publish_debug_topics_ = - this->declare_parameter("publish_debug_topics", false); - gain_acc = this->declare_parameter("gain_acc", 0.01); - gain_mag = this->declare_parameter("gain_mag", 0.01); + // set "Not Dynamically Reconfigurable Parameters" + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.read_only = true; + + fixed_frame_ = + this->declare_parameter("fixed_frame", "odom", descriptor); + use_mag_ = this->declare_parameter("use_mag", false, descriptor); + publish_tf_ = + this->declare_parameter("publish_tf", false, descriptor); + reverse_tf_ = + this->declare_parameter("reverse_tf", false, descriptor); + constant_dt_ = + this->declare_parameter("constant_dt", 0.0, descriptor); + publish_debug_topics_ = this->declare_parameter( + "publish_debug_topics", false, descriptor); + gain_acc = this->declare_parameter("gain_acc", 0.01, descriptor); + gain_mag = this->declare_parameter("gain_mag", 0.01, descriptor); do_bias_estimation = - this->declare_parameter("do_bias_estimation", true); - bias_alpha = this->declare_parameter("bias_alpha", 0.01); - do_adaptive_gain = this->declare_parameter("do_adaptive_gain", true); + this->declare_parameter("do_bias_estimation", true, descriptor); + bias_alpha = + this->declare_parameter("bias_alpha", 0.01, descriptor); + do_adaptive_gain = + this->declare_parameter("do_adaptive_gain", true, descriptor); + orientation_stddev = this->declare_parameter("orientation_stddev", 0.0); orientation_variance_ = orientation_stddev * orientation_stddev; diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index a5773312..33dab25f 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/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") { From 3ebe5a2985cba09981201f8be7f8c16ecb09d5e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 26 Apr 2024 13:08:02 +0200 Subject: [PATCH 02/17] CI: Use pre-commit from apt repositories instead of pip (#198) pre-commit is available as an APT package starting with Ubuntu 22.04. This commit avoids the following error: WARNING: The directory '/github/home/.cache/pip' or its parent directory is not owned or is not writable by the current user. The cache has been disabled. Check the permissions and owner of that directory. If executing pip with sudo, you should use sudo's -H flag. error: externally-managed-environment x This environment is externally managed --> To install Python packages system-wide, try apt install python3-xyz, where xyz is the package you are trying to install. If you wish to install a non-Debian-packaged Python package, create a virtual environment using python3 -m venv path/to/venv. Then use path/to/venv/bin/python and path/to/venv/bin/pip. Make sure you have python3-full installed. If you wish to install a non-Debian packaged Python application, it may be easiest to use pipx install xyz, which will manage a virtual environment for you. Make sure you have pipx installed. See /usr/share/doc/python3.12/README.venv for more information. note: If you believe this is a mistake, please contact your Python installation or OS distribution provider. You can override this, at the risk of breaking your Python installation or OS, by passing --break-system-packages. hint: See PEP 668 for the detailed specification. --- .github/workflows/github-actions.yml | 4 +--- README.md | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index 45ba9284..081219d7 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -13,9 +13,7 @@ jobs: - 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 with: diff --git a/README.md b/README.md index 4b74181f..e9219a60 100644 --- a/README.md +++ b/README.md @@ -77,10 +77,10 @@ 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: From 6e0a278f9b3623fc2163e8960593063fc66c6311 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 26 Apr 2024 13:06:37 +0200 Subject: [PATCH 03/17] CI: Update to actions/checkout@v4 This fixes the following warning: Node.js 16 actions are deprecated. Please update the following actions to use Node.js 20: actions/checkout@v3. For more information see: https://github.blog/changelog/2023-09-22-github-actions-transitioning-from-node-16-to-node-20/. --- .github/workflows/github-actions.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index 081219d7..d0c376cc 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -15,7 +15,7 @@ jobs: apt-get update 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 From 14e7079bac238acc0d4ab8aece56eaf011898133 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 26 Apr 2024 13:11:40 +0200 Subject: [PATCH 04/17] CI: Add tmate debugging This commit adds the option of running the GitHub workflow manually (workflow_dispatch) and optionally enable tmate for SSH debugging. --- .github/workflows/github-actions.yml | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index d0c376cc..435ed1a1 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -1,5 +1,14 @@ 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: @@ -10,6 +19,12 @@ jobs: 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 From 102c6d3602d039f45a877658719e21f07dc965d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 26 Apr 2024 13:11:56 +0200 Subject: [PATCH 05/17] CI: Add "iron" to test matrix --- .github/workflows/github-actions.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index 435ed1a1..07a608d4 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -13,7 +13,7 @@ jobs: build: strategy: matrix: - rosdistro: [humble, rolling] + rosdistro: [humble, iron, rolling] fail-fast: false runs-on: ubuntu-latest container: From e4c2fcec84548789310d87103dd830a9cc0b8021 Mon Sep 17 00:00:00 2001 From: Tamaki Nishino Date: Fri, 26 Apr 2024 21:23:14 +0900 Subject: [PATCH 06/17] Show remapped topic names (#196) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This PR, similar to #192, changes the topic names in the warning message to remapped ones in order to make the message more understandable. --------- Signed-off-by: Tamaki Nishino Co-authored-by: Martin Günther --- imu_filter_madgwick/src/imu_filter_ros.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index 33dab25f..5828c162 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -543,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" From dea62f5ee958745f5d32e80b3bc498ac6b976487 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 26 Apr 2024 15:53:25 +0200 Subject: [PATCH 07/17] Update changelogs --- imu_complementary_filter/CHANGELOG.rst | 5 +++++ imu_filter_madgwick/CHANGELOG.rst | 6 ++++++ imu_tools/CHANGELOG.rst | 3 +++ rviz_imu_plugin/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/imu_complementary_filter/CHANGELOG.rst b/imu_complementary_filter/CHANGELOG.rst index d2aede66..051bf5d1 100644 --- a/imu_complementary_filter/CHANGELOG.rst +++ b/imu_complementary_filter/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_complementary_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Set read-only parameters as read_only (`#185 `_) +* Contributors: Christoph Fröhlich + 2.1.3 (2022-12-07) ------------------ * complementary: Build shared library diff --git a/imu_filter_madgwick/CHANGELOG.rst b/imu_filter_madgwick/CHANGELOG.rst index 79053c0a..caadb3dc 100644 --- a/imu_filter_madgwick/CHANGELOG.rst +++ b/imu_filter_madgwick/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_filter_madgwick ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Show remapped topic names (`#196 `_) +* Set read-only parameters as read_only (`#185 `_) +* Contributors: Christoph Fröhlich, Tamaki Nishino + 2.1.3 (2022-12-07) ------------------ * Update CMakeLists to use targets diff --git a/imu_tools/CHANGELOG.rst b/imu_tools/CHANGELOG.rst index 4cddca04..f43bcc8c 100644 --- a/imu_tools/CHANGELOG.rst +++ b/imu_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.3 (2022-12-07) ------------------ diff --git a/rviz_imu_plugin/CHANGELOG.rst b/rviz_imu_plugin/CHANGELOG.rst index 9c377919..636cd16c 100644 --- a/rviz_imu_plugin/CHANGELOG.rst +++ b/rviz_imu_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rviz_imu_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.3 (2022-12-07) ------------------ From 8cf9ce38ac31b44e6fb83deaac9049cf18c53242 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 26 Apr 2024 15:53:55 +0200 Subject: [PATCH 08/17] 2.1.4 --- imu_complementary_filter/CHANGELOG.rst | 4 ++-- imu_complementary_filter/package.xml | 2 +- imu_filter_madgwick/CHANGELOG.rst | 4 ++-- imu_filter_madgwick/package.xml | 2 +- imu_tools/CHANGELOG.rst | 4 ++-- imu_tools/package.xml | 2 +- rviz_imu_plugin/CHANGELOG.rst | 4 ++-- rviz_imu_plugin/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/imu_complementary_filter/CHANGELOG.rst b/imu_complementary_filter/CHANGELOG.rst index 051bf5d1..f7ee44ef 100644 --- a/imu_complementary_filter/CHANGELOG.rst +++ b/imu_complementary_filter/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_complementary_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.4 (2024-04-26) +------------------ * Set read-only parameters as read_only (`#185 `_) * Contributors: Christoph Fröhlich diff --git a/imu_complementary_filter/package.xml b/imu_complementary_filter/package.xml index d3c79511..9cc2adf3 100644 --- a/imu_complementary_filter/package.xml +++ b/imu_complementary_filter/package.xml @@ -1,7 +1,7 @@ imu_complementary_filter - 2.1.3 + 2.1.4 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 . Martin Günther diff --git a/imu_filter_madgwick/CHANGELOG.rst b/imu_filter_madgwick/CHANGELOG.rst index caadb3dc..24350f5d 100644 --- a/imu_filter_madgwick/CHANGELOG.rst +++ b/imu_filter_madgwick/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_filter_madgwick ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.4 (2024-04-26) +------------------ * Show remapped topic names (`#196 `_) * Set read-only parameters as read_only (`#185 `_) * Contributors: Christoph Fröhlich, Tamaki Nishino diff --git a/imu_filter_madgwick/package.xml b/imu_filter_madgwick/package.xml index 765b9c36..2a00f53f 100644 --- a/imu_filter_madgwick/package.xml +++ b/imu_filter_madgwick/package.xml @@ -2,7 +2,7 @@ imu_filter_madgwick - 2.1.3 + 2.1.4 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. diff --git a/imu_tools/CHANGELOG.rst b/imu_tools/CHANGELOG.rst index f43bcc8c..3c5f76e8 100644 --- a/imu_tools/CHANGELOG.rst +++ b/imu_tools/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.4 (2024-04-26) +------------------ 2.1.3 (2022-12-07) ------------------ diff --git a/imu_tools/package.xml b/imu_tools/package.xml index 58822003..3276a959 100644 --- a/imu_tools/package.xml +++ b/imu_tools/package.xml @@ -1,6 +1,6 @@ imu_tools - 2.1.3 + 2.1.4 Various tools for IMU devices diff --git a/rviz_imu_plugin/CHANGELOG.rst b/rviz_imu_plugin/CHANGELOG.rst index 636cd16c..971e80f2 100644 --- a/rviz_imu_plugin/CHANGELOG.rst +++ b/rviz_imu_plugin/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rviz_imu_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.4 (2024-04-26) +------------------ 2.1.3 (2022-12-07) ------------------ diff --git a/rviz_imu_plugin/package.xml b/rviz_imu_plugin/package.xml index a04e259c..e8ad623a 100644 --- a/rviz_imu_plugin/package.xml +++ b/rviz_imu_plugin/package.xml @@ -2,7 +2,7 @@ rviz_imu_plugin - 2.1.3 + 2.1.4 RVIZ plugin for IMU visualization From f089c6dd47458d33791bdf046acc2e623c72a898 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 27 Sep 2024 20:05:07 +0200 Subject: [PATCH 09/17] Add jazzy to CI (#208) --- .github/workflows/github-actions.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index 07a608d4..f96a6f43 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -13,7 +13,7 @@ jobs: build: strategy: matrix: - rosdistro: [humble, iron, rolling] + rosdistro: [humble, iron, jazzy, rolling] fail-fast: false runs-on: ubuntu-latest container: From 11bf90042483e1f96aa9c54ef098ae32af607a8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Tue, 1 Oct 2024 12:49:40 +0200 Subject: [PATCH 10/17] Add QoS overriding options (#207) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * add yaml file with parameters and include it in CMakeLists * include the yaml in the launch file * add qos overriding options and change topic namespaces * fix typos in param names * add imu/mag topic params to yaml config file * format launch.py file * format files with clang-format --------- Signed-off-by: Aleksander Szymański --- imu_complementary_filter/CMakeLists.txt | 2 +- .../config/filter_config.yaml | 28 +++++++++++++++++ .../launch/complementary_filter.launch.py | 30 +++++++++---------- .../src/complementary_filter_ros.cpp | 16 ++++++++-- 4 files changed, 56 insertions(+), 20 deletions(-) create mode 100644 imu_complementary_filter/config/filter_config.yaml diff --git a/imu_complementary_filter/CMakeLists.txt b/imu_complementary_filter/CMakeLists.txt index 645fd0b9..cd1d015a 100644 --- a/imu_complementary_filter/CMakeLists.txt +++ b/imu_complementary_filter/CMakeLists.txt @@ -63,7 +63,7 @@ install( DESTINATION include ) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/imu_complementary_filter/config/filter_config.yaml b/imu_complementary_filter/config/filter_config.yaml new file mode 100644 index 00000000..2f3ac646 --- /dev/null +++ b/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 diff --git a/imu_complementary_filter/launch/complementary_filter.launch.py b/imu_complementary_filter/launch/complementary_filter.launch.py index b6cce836..9b764ba0 100644 --- a/imu_complementary_filter/launch/complementary_filter.launch.py +++ b/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 diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index 96f1bdaa..89397c9d 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -64,17 +64,27 @@ ComplementaryFilterROS::ComplementaryFilterROS() if (filter_.getDoBiasEstimation()) { state_publisher_ = this->create_publisher( - "/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_)); From 98006330e86727f6d6c4aa42140d907f4948d7c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 1 Oct 2024 13:13:24 +0200 Subject: [PATCH 11/17] Split humble/iron/jazzy from rolling (#209) * removed rolling from the CI config * this is in preparation of fixing the deprecated header warnings in rolling, which is backward-incompatible, so splitting the branches is the only way (see #207 for discussion) * from now on, the 2.2.* releases will go to `rolling` and the 2.1.* releases to the other branches. --- .github/workflows/github-actions.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index f96a6f43..8cb8e8fb 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -13,7 +13,7 @@ jobs: build: strategy: matrix: - rosdistro: [humble, iron, jazzy, rolling] + rosdistro: [humble, iron, jazzy] fail-fast: false runs-on: ubuntu-latest container: From 9ceb361073b9258bcf30188dbef7f1cee0ed4ea3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 1 Oct 2024 13:22:26 +0200 Subject: [PATCH 12/17] Update changelogs --- imu_complementary_filter/CHANGELOG.rst | 5 +++++ imu_filter_madgwick/CHANGELOG.rst | 3 +++ imu_tools/CHANGELOG.rst | 3 +++ rviz_imu_plugin/CHANGELOG.rst | 3 +++ 4 files changed, 14 insertions(+) diff --git a/imu_complementary_filter/CHANGELOG.rst b/imu_complementary_filter/CHANGELOG.rst index f7ee44ef..2ec56d41 100644 --- a/imu_complementary_filter/CHANGELOG.rst +++ b/imu_complementary_filter/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_complementary_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add QoS overriding options (`#207 `_) +* Contributors: Aleksander Szymański + 2.1.4 (2024-04-26) ------------------ * Set read-only parameters as read_only (`#185 `_) diff --git a/imu_filter_madgwick/CHANGELOG.rst b/imu_filter_madgwick/CHANGELOG.rst index 24350f5d..6621454a 100644 --- a/imu_filter_madgwick/CHANGELOG.rst +++ b/imu_filter_madgwick/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_filter_madgwick ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.4 (2024-04-26) ------------------ * Show remapped topic names (`#196 `_) diff --git a/imu_tools/CHANGELOG.rst b/imu_tools/CHANGELOG.rst index 3c5f76e8..922d7635 100644 --- a/imu_tools/CHANGELOG.rst +++ b/imu_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.4 (2024-04-26) ------------------ diff --git a/rviz_imu_plugin/CHANGELOG.rst b/rviz_imu_plugin/CHANGELOG.rst index 971e80f2..91c65bb9 100644 --- a/rviz_imu_plugin/CHANGELOG.rst +++ b/rviz_imu_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rviz_imu_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.4 (2024-04-26) ------------------ From d4c05e378a6e8aef4a46c726e92970c0d8f70e5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 1 Oct 2024 13:22:48 +0200 Subject: [PATCH 13/17] 2.1.5 --- imu_complementary_filter/CHANGELOG.rst | 4 ++-- imu_complementary_filter/package.xml | 2 +- imu_filter_madgwick/CHANGELOG.rst | 4 ++-- imu_filter_madgwick/package.xml | 2 +- imu_tools/CHANGELOG.rst | 4 ++-- imu_tools/package.xml | 2 +- rviz_imu_plugin/CHANGELOG.rst | 4 ++-- rviz_imu_plugin/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/imu_complementary_filter/CHANGELOG.rst b/imu_complementary_filter/CHANGELOG.rst index 2ec56d41..14fd9ae5 100644 --- a/imu_complementary_filter/CHANGELOG.rst +++ b/imu_complementary_filter/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_complementary_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.5 (2024-10-01) +------------------ * Add QoS overriding options (`#207 `_) * Contributors: Aleksander Szymański diff --git a/imu_complementary_filter/package.xml b/imu_complementary_filter/package.xml index 9cc2adf3..2ec275b5 100644 --- a/imu_complementary_filter/package.xml +++ b/imu_complementary_filter/package.xml @@ -1,7 +1,7 @@ imu_complementary_filter - 2.1.4 + 2.1.5 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 . Martin Günther diff --git a/imu_filter_madgwick/CHANGELOG.rst b/imu_filter_madgwick/CHANGELOG.rst index 6621454a..2fe394c8 100644 --- a/imu_filter_madgwick/CHANGELOG.rst +++ b/imu_filter_madgwick/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_filter_madgwick ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.5 (2024-10-01) +------------------ 2.1.4 (2024-04-26) ------------------ diff --git a/imu_filter_madgwick/package.xml b/imu_filter_madgwick/package.xml index 2a00f53f..cfc16b94 100644 --- a/imu_filter_madgwick/package.xml +++ b/imu_filter_madgwick/package.xml @@ -2,7 +2,7 @@ imu_filter_madgwick - 2.1.4 + 2.1.5 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. diff --git a/imu_tools/CHANGELOG.rst b/imu_tools/CHANGELOG.rst index 922d7635..21e4dcf4 100644 --- a/imu_tools/CHANGELOG.rst +++ b/imu_tools/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.5 (2024-10-01) +------------------ 2.1.4 (2024-04-26) ------------------ diff --git a/imu_tools/package.xml b/imu_tools/package.xml index 3276a959..1981729f 100644 --- a/imu_tools/package.xml +++ b/imu_tools/package.xml @@ -1,6 +1,6 @@ imu_tools - 2.1.4 + 2.1.5 Various tools for IMU devices diff --git a/rviz_imu_plugin/CHANGELOG.rst b/rviz_imu_plugin/CHANGELOG.rst index 91c65bb9..b7176593 100644 --- a/rviz_imu_plugin/CHANGELOG.rst +++ b/rviz_imu_plugin/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rviz_imu_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.5 (2024-10-01) +------------------ 2.1.4 (2024-04-26) ------------------ diff --git a/rviz_imu_plugin/package.xml b/rviz_imu_plugin/package.xml index e8ad623a..68d12b2f 100644 --- a/rviz_imu_plugin/package.xml +++ b/rviz_imu_plugin/package.xml @@ -2,7 +2,7 @@ rviz_imu_plugin - 2.1.4 + 2.1.5 RVIZ plugin for IMU visualization From 69261f53be33cb489022cdedf217c7358905d888 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Wed, 7 May 2025 13:20:52 +0200 Subject: [PATCH 14/17] CI: Drop iron from rosdistro matrix Iron is end-of-life, so its rosdistro keys aren't resolved any more. --- .github/workflows/github-actions.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/github-actions.yml b/.github/workflows/github-actions.yml index 8cb8e8fb..ca9a05e1 100644 --- a/.github/workflows/github-actions.yml +++ b/.github/workflows/github-actions.yml @@ -13,7 +13,7 @@ jobs: build: strategy: matrix: - rosdistro: [humble, iron, jazzy] + rosdistro: [humble, jazzy] fail-fast: false runs-on: ubuntu-latest container: From 7cdf49ec257e8b515b9222dc16cd52da28751e71 Mon Sep 17 00:00:00 2001 From: wentywenty <63338034+wentywenty@users.noreply.github.com> Date: Wed, 14 Jan 2026 03:42:18 +0800 Subject: [PATCH 15/17] Fixed a static library linking issue with the imu_complementary_filter package on Windows. (#218) --- imu_complementary_filter/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/imu_complementary_filter/CMakeLists.txt b/imu_complementary_filter/CMakeLists.txt index cd1d015a..91cb66d9 100644 --- a/imu_complementary_filter/CMakeLists.txt +++ b/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 From 9d79d7c389efb1865fdb00868a50e265c81c196f Mon Sep 17 00:00:00 2001 From: Kris <155166879+kristkis@users.noreply.github.com> Date: Tue, 28 Apr 2026 19:28:41 +0400 Subject: [PATCH 16/17] rviz_imu_plugin: fix invisible-arrow and stale-direction bugs in ImuAccVisual (#228) Bugs fixed in this commit: * Invisible acceleration arrow for raw-IMU publishers that don't estimate orientation (issues #86, #219 likely root cause). * "Derotate Acceleration" checkbox didn't visibly change the arrow until a new message arrived. , Aliensense Signed-off-by: kristkis , Aliensense Signed-off-by: Claude Co-authored-by: Claude --- rviz_imu_plugin/src/imu_acc_visual.cpp | 75 ++++++++++++++++++++------ rviz_imu_plugin/src/imu_acc_visual.h | 8 +++ 2 files changed, 67 insertions(+), 16 deletions(-) diff --git a/rviz_imu_plugin/src/imu_acc_visual.cpp b/rviz_imu_plugin/src/imu_acc_visual.cpp index 88abac00..6b20edda 100644 --- a/rviz_imu_plugin/src/imu_acc_visual.cpp +++ b/rviz_imu_plugin/src/imu_acc_visual.cpp @@ -32,6 +32,10 @@ #include #include "imu_acc_visual.h" + +#include +#include +#include #include namespace rviz_imu_plugin { @@ -39,6 +43,7 @@ namespace rviz_imu_plugin { ImuAccVisual::ImuAccVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) : acc_vector_(NULL), + quat_valid_(true), arrow_length_(9.81), arrow_radius_(0.50), head_length_(1.00), @@ -92,25 +97,46 @@ void ImuAccVisual::hide() void ImuAccVisual::setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - direction_ = + raw_acc_ = Ogre::Vector3(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); + arrow_length_ = raw_acc_.length(); - // Rotate the acceleration vector by the IMU orientation. This makes - // sense since the visualization of the IMU is also rotated by the - // orientation. In this way, both appear in the inertial frame. - if (derotated_) + if (checkQuaternionValidity(msg)) { - Ogre::Quaternion orientation(msg->orientation.w, msg->orientation.x, - msg->orientation.y, msg->orientation.z); - - direction_ = orientation * direction_; + if (!quat_valid_) + { + RVIZ_COMMON_LOG_INFO_STREAM( + "rviz_imu_plugin got valid quaternion, " + "applying acceleration derotation"); + quat_valid_ = true; + } + orientation_ = Ogre::Quaternion(msg->orientation.w, msg->orientation.x, + msg->orientation.y, msg->orientation.z); + } else + { + if (quat_valid_) + { + RVIZ_COMMON_LOG_WARNING_STREAM( + "rviz_imu_plugin got invalid quaternion (" + << msg->orientation.w << "," << msg->orientation.x << "," + << msg->orientation.y << "," << msg->orientation.z + << "); skipping acceleration derotation"); + quat_valid_ = false; + } + orientation_ = Ogre::Quaternion(); } - arrow_length_ = - sqrt(msg->linear_acceleration.x * msg->linear_acceleration.x + - msg->linear_acceleration.y * msg->linear_acceleration.y + - msg->linear_acceleration.z * msg->linear_acceleration.z); + updateDirection(); +} + +void ImuAccVisual::updateDirection() +{ + direction_ = raw_acc_; + if (derotated_ && quat_valid_) + { + direction_ = orientation_ * direction_; + } if (acc_vector_) { @@ -150,9 +176,7 @@ void ImuAccVisual::setAlpha(float alpha) void ImuAccVisual::setDerotated(bool derotated) { derotated_ = derotated; - if (acc_vector_) - acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), - alpha_); + updateDirection(); } void ImuAccVisual::setFramePosition(const Ogre::Vector3& position) @@ -165,4 +189,23 @@ void ImuAccVisual::setFrameOrientation(const Ogre::Quaternion& orientation) frame_node_->setOrientation(orientation); } +bool ImuAccVisual::checkQuaternionValidity( + const sensor_msgs::msg::Imu::ConstSharedPtr msg) +{ + // REP-145: orientation_covariance[0] == -1 signals "not estimated". + if (msg->orientation_covariance[0] == -1.0) + { + return false; + } + double x = msg->orientation.x, y = msg->orientation.y, + z = msg->orientation.z, w = msg->orientation.w; + // Near-zero length indicates the default (0, 0, 0, 0) quaternion, + // which would silently zero out any vector multiplication. + if (std::sqrt(x * x + y * y + z * z + w * w) < 0.0001) + { + return false; + } + return true; +} + } // namespace rviz_imu_plugin diff --git a/rviz_imu_plugin/src/imu_acc_visual.h b/rviz_imu_plugin/src/imu_acc_visual.h index ff2751a0..3b84ad20 100644 --- a/rviz_imu_plugin/src/imu_acc_visual.h +++ b/rviz_imu_plugin/src/imu_acc_visual.h @@ -91,11 +91,19 @@ class ImuAccVisual private: void create(); + void updateDirection(); + + static bool checkQuaternionValidity( + const sensor_msgs::msg::Imu::ConstSharedPtr msg); rviz_rendering::Arrow* acc_vector_; Ogre::Vector3 direction_; // computed from IMU message + Ogre::Vector3 raw_acc_; + Ogre::Quaternion orientation_; + bool quat_valid_; + float arrow_length_; // computed from IMU message float arrow_radius_; float head_length_; From 2aea4ac9420aab2cf6c450b5003fc8bf07c84762 Mon Sep 17 00:00:00 2001 From: Manan Kharwar <44316521+manankharwar@users.noreply.github.com> Date: Mon, 4 May 2026 11:19:16 -0400 Subject: [PATCH 17/17] docs: add next-steps section for state estimation after IMU filtering (#232) Refs #231 (suggested and approved by maintainer mintar) Co-authored-by: Manan Kharwar --- README.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/README.md b/README.md index e9219a60..9b8e5f0d 100644 --- a/README.md +++ b/README.md @@ -72,6 +72,26 @@ 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 ----------------------------