├── .dockerfilelintrc ├── rviz_imu_plugin ├── rosdoc.yaml ├── rviz_imu_plugin.png ├── plugin_description.xml ├── package.xml ├── CMakeLists.txt ├── src │ ├── imu_axes_visual.h │ ├── mag_visual.h │ ├── mag_display.h │ ├── imu_acc_visual.h │ ├── imu_orientation_visual.h │ ├── mag_visual.cpp │ ├── imu_axes_visual.cpp │ ├── mag_display.cpp │ ├── imu_acc_visual.cpp │ ├── imu_display.h │ ├── imu_orientation_visual.cpp │ └── imu_display.cpp └── CHANGELOG.rst ├── pyproject.toml ├── imu_filter_madgwick ├── sample │ ├── ardrone_imu.bag │ ├── sparkfun_razor.bag │ └── phidgets_imu_upside_down.bag ├── include │ └── imu_filter_madgwick │ │ ├── world_frame.h │ │ ├── stateless_orientation.h │ │ ├── visibility_control.h │ │ ├── imu_filter.h │ │ ├── base_node.hpp │ │ └── imu_filter_ros.h ├── test │ ├── CMakeLists.txt │ ├── test_helpers.h │ ├── madgwick_test.cpp │ └── stateless_orientation_test.cpp ├── config │ └── imu_filter.yaml ├── launch │ ├── imu_filter.launch.py │ └── imu_filter_component.launch.py ├── package.xml ├── src │ ├── imu_filter_node.cpp │ ├── stateless_orientation.cpp │ └── imu_filter.cpp ├── CMakeLists.txt └── CHANGELOG.rst ├── imu_tools ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── LICENSE ├── .clang-format ├── .gitignore ├── imu_complementary_filter ├── launch │ └── complementary_filter.launch.py ├── config │ └── filter_config.yaml ├── package.xml ├── src │ ├── complementary_filter_node.cpp │ └── complementary_filter_ros.cpp ├── CMakeLists.txt ├── include │ └── imu_complementary_filter │ │ ├── complementary_filter_ros.h │ │ └── complementary_filter.h └── CHANGELOG.rst ├── Dockerfile-foxy ├── Dockerfile-galactic ├── .pre-commit-config.yaml ├── LICENSE.bsd ├── .github └── workflows │ └── github-actions.yml └── README.md /.dockerfilelintrc: -------------------------------------------------------------------------------- 1 | rules: 2 | apt-get-update_require_install: off 3 | -------------------------------------------------------------------------------- /rviz_imu_plugin/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: src/doc 3 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | line-length = 120 3 | target-version = ['py38'] 4 | skip-string-normalization = true 5 | -------------------------------------------------------------------------------- /rviz_imu_plugin/rviz_imu_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CCNYRoboticsLab/imu_tools/HEAD/rviz_imu_plugin/rviz_imu_plugin.png -------------------------------------------------------------------------------- /imu_filter_madgwick/sample/ardrone_imu.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CCNYRoboticsLab/imu_tools/HEAD/imu_filter_madgwick/sample/ardrone_imu.bag -------------------------------------------------------------------------------- /imu_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(imu_tools) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /imu_filter_madgwick/sample/sparkfun_razor.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CCNYRoboticsLab/imu_tools/HEAD/imu_filter_madgwick/sample/sparkfun_razor.bag -------------------------------------------------------------------------------- /imu_filter_madgwick/sample/phidgets_imu_upside_down.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CCNYRoboticsLab/imu_tools/HEAD/imu_filter_madgwick/sample/phidgets_imu_upside_down.bag -------------------------------------------------------------------------------- /imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_FILTER_MADGWICK_WORLD_FRAME_H_ 2 | #define IMU_FILTER_MADGWICK_WORLD_FRAME_H_ 3 | 4 | namespace WorldFrame { 5 | enum WorldFrame { ENU, NED, NWU }; 6 | } 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The imu_filter_madgwick package is licensed under the GNU General Public Licence v3 or later. 2 | See LICENSE.gplv3 for the text of the license. 3 | 4 | The rest of the code in this repository is licensed under the 3-clause BSD license. 5 | See LICENSE.bsd for the text of the license. 6 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | Language: Cpp 4 | AccessModifierOffset: -2 5 | AllowShortFunctionsOnASingleLine: false 6 | BreakBeforeBraces: Custom 7 | BraceWrapping: 8 | AfterControlStatement: true 9 | AfterClass: true 10 | AfterFunction: true 11 | SplitEmptyFunction: true 12 | IndentWidth: 4 13 | SortIncludes: false 14 | -------------------------------------------------------------------------------- /imu_filter_madgwick/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT WIN32) 2 | ament_add_gtest(madgwick_test madgwick_test.cpp) 3 | target_link_libraries(madgwick_test imu_filter_madgwick) 4 | 5 | ament_add_gtest(stateless_orientation_test stateless_orientation_test.cpp) 6 | target_link_libraries(stateless_orientation_test imu_filter_madgwick) 7 | endif() 8 | -------------------------------------------------------------------------------- /imu_filter_madgwick/config/imu_filter.yaml: -------------------------------------------------------------------------------- 1 | imu_filter: 2 | ros__parameters: 3 | stateless: false 4 | use_mag: true 5 | publish_tf: true 6 | reverse_tf: false 7 | fixed_frame: "odom" 8 | constant_dt: 0.0 9 | publish_debug_topics: false 10 | world_frame: "enu" 11 | gain: 0.1 12 | zeta: 0.0 13 | mag_bias_x: 0.0 14 | mag_bias_y: 0.0 15 | mag_bias_z: 0.0 16 | orientation_stddev: 0.0 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | .pydevproject 4 | cmake_install.cmake 5 | bin/ 6 | build/ 7 | lib/ 8 | *.cfgc 9 | imu_filter_madgwick/cfg/cpp/ 10 | imu_filter_madgwick/docs/ImuFilterMadgwickConfig-usage.dox 11 | imu_filter_madgwick/docs/ImuFilterMadgwickConfig.dox 12 | imu_filter_madgwick/docs/ImuFilterMadgwickConfig.wikidoc 13 | imu_filter_madgwick/src/imu_filter_madgwick/__init__.py 14 | imu_filter_madgwick/src/imu_filter_madgwick/cfg/ 15 | *~ 16 | -------------------------------------------------------------------------------- /imu_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | imu_tools 3 | 2.2.1 4 | 5 | Various tools for IMU devices 6 | 7 | Martin Günther 8 | BSD, GPL 9 | 10 | http://ros.org/wiki/imu_tools 11 | 12 | ament_cmake 13 | 14 | imu_complementary_filter 15 | imu_filter_madgwick 16 | rviz_imu_plugin 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /imu_complementary_filter/launch/complementary_filter.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch import LaunchDescription 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description(): 8 | ld = LaunchDescription() 9 | 10 | config = os.path.join(get_package_share_directory('imu_complementary_filter'), 'config', 'filter_config.yaml') 11 | 12 | node = Node( 13 | package='imu_complementary_filter', 14 | executable='complementary_filter_node', 15 | name='complementary_filter_gain_node', 16 | output='screen', 17 | parameters=[config], 18 | ) 19 | ld.add_action(node) 20 | return ld 21 | -------------------------------------------------------------------------------- /imu_complementary_filter/config/filter_config.yaml: -------------------------------------------------------------------------------- 1 | complementary_filter_gain_node: 2 | ros__parameters: 3 | gain_acc: 0.01 4 | gain_mag: 0.01 5 | bias_alpha: 0.01 6 | do_bias_estimation: true 7 | do_adaptive_gain: true 8 | use_mag: false 9 | fixed_frame: "odom" 10 | publish_tf: false 11 | reverse_tf: false 12 | constant_dt: 0.0 13 | publish_debug_topics: false 14 | 15 | qos_overrides: 16 | /imu/data_raw: 17 | subscription: 18 | depth: 10 19 | durability: volatile 20 | history: keep_last 21 | reliability: reliable 22 | 23 | /imu/mag: 24 | subscription: 25 | depth: 10 26 | durability: volatile 27 | history: keep_last 28 | reliability: reliable 29 | -------------------------------------------------------------------------------- /imu_filter_madgwick/launch/imu_filter.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | import launch.actions 4 | import launch.substitutions 5 | import launch_ros.actions 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | config_dir = os.path.join(get_package_share_directory('imu_filter_madgwick'), 'config') 12 | 13 | return launch.LaunchDescription( 14 | [ 15 | launch_ros.actions.Node( 16 | package='imu_filter_madgwick', 17 | executable='imu_filter_madgwick_node', 18 | name='imu_filter', 19 | output='screen', 20 | parameters=[os.path.join(config_dir, 'imu_filter.yaml')], 21 | ) 22 | ] 23 | ) 24 | -------------------------------------------------------------------------------- /rviz_imu_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | Displays the orientation and acceleration components of sensor_msgs/Imu messages. 9 | 10 | sensor_msgs/msg/Imu 11 | 12 | 13 | 16 | 17 | 18 | Displays the orientation of sensor_msgs/MagneticField messages. 19 | 20 | sensor_msgs/msg/MagneticField 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /Dockerfile-foxy: -------------------------------------------------------------------------------- 1 | FROM ros:foxy-ros-core 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | build-essential file clang-format python3-colcon-common-extensions python3-rosdep \ 5 | && rm -rf /var/lib/apt/lists/* 6 | 7 | # Install pre-commit hooks to /root/.cache/pre-commit/ 8 | RUN apt-get update -qq \ 9 | && apt-get install -y -qq --no-install-recommends git python3-pip clang-format python3-catkin-lint \ 10 | && rm -rf /var/lib/apt/lists/* 11 | RUN pip3 install pre-commit 12 | RUN mkdir -p /tmp/pre-commit 13 | COPY .pre-commit-config.yaml /tmp/pre-commit/ 14 | RUN cd /tmp/pre-commit \ 15 | && git init \ 16 | && pre-commit install-hooks \ 17 | && rm -rf /tmp/pre-commit 18 | 19 | # Create ROS workspace 20 | COPY . /ws/src/imu_tools 21 | WORKDIR /ws 22 | 23 | # Use rosdep to install all dependencies (including ROS itself) 24 | RUN rosdep init && rosdep update && apt-get update && rosdep install --from-paths src -i -y --rosdistro foxy \ 25 | && rm -rf /var/lib/apt/lists/* 26 | 27 | RUN /bin/bash -c "source /opt/ros/foxy/setup.bash && \ 28 | colcon build --parallel-workers 1 && \ 29 | colcon test --parallel-workers 1" 30 | -------------------------------------------------------------------------------- /Dockerfile-galactic: -------------------------------------------------------------------------------- 1 | FROM ros:galactic-ros-core 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | build-essential file clang-format python3-colcon-common-extensions python3-rosdep \ 5 | && rm -rf /var/lib/apt/lists/* 6 | 7 | # Install pre-commit hooks to /root/.cache/pre-commit/ 8 | RUN apt-get update -qq \ 9 | && apt-get install -y -qq --no-install-recommends git python3-pip clang-format python3-catkin-lint \ 10 | && rm -rf /var/lib/apt/lists/* 11 | RUN pip3 install pre-commit 12 | RUN mkdir -p /tmp/pre-commit 13 | COPY .pre-commit-config.yaml /tmp/pre-commit/ 14 | RUN cd /tmp/pre-commit \ 15 | && git init \ 16 | && pre-commit install-hooks \ 17 | && rm -rf /tmp/pre-commit 18 | 19 | # Create ROS workspace 20 | COPY . /ws/src/imu_tools 21 | WORKDIR /ws 22 | 23 | # Use rosdep to install all dependencies (including ROS itself) 24 | RUN rosdep init && rosdep update && apt-get update && rosdep install --from-paths src -i -y --rosdistro galactic \ 25 | && rm -rf /var/lib/apt/lists/* 26 | 27 | RUN /bin/bash -c "source /opt/ros/galactic/setup.bash && \ 28 | colcon build --parallel-workers 1 && \ 29 | colcon test --parallel-workers 1" 30 | -------------------------------------------------------------------------------- /imu_complementary_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_complementary_filter 4 | 2.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 . 6 | 7 | Martin Günther 8 | BSD 9 | 10 | http://www.mdpi.com/1424-8220/15/8/19302 11 | Roberto G. Valenti 12 | 13 | ament_cmake 14 | geometry_msgs 15 | message_filters 16 | rclcpp 17 | sensor_msgs 18 | std_msgs 19 | tf2 20 | tf2_ros 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /imu_filter_madgwick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | imu_filter_madgwick 5 | 2.2.1 6 | 7 | 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. 8 | 9 | GPL 10 | http://ros.org/wiki/imu_filter_madgwick 11 | Ivan Dryanovski 12 | Martin Günther 13 | 14 | ament_cmake 15 | 16 | rclcpp 17 | rclcpp_action 18 | rclcpp_lifecycle 19 | visualization_msgs 20 | nav_msgs 21 | geometry_msgs 22 | builtin_interfaces 23 | tf2_ros 24 | tf2_geometry_msgs 25 | sensor_msgs 26 | 27 | ament_cmake_gtest 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /rviz_imu_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rviz_imu_plugin 5 | 2.2.1 6 | 7 | RVIZ plugin for IMU visualization 8 | 9 | BSD 10 | http://ros.org/wiki/rviz_imu_plugin 11 | Ivan Dryanovski 12 | Martin Günther 13 | 14 | ament_cmake 15 | 16 | qtbase5-dev 17 | 18 | message_filters 19 | pluginlib 20 | rclcpp 21 | rviz_common 22 | rviz_ogre_vendor 23 | rviz_rendering 24 | sensor_msgs 25 | tf2 26 | tf2_ros 27 | 28 | libqt5-core 29 | libqt5-gui 30 | libqt5-opengl 31 | libqt5-widgets 32 | 33 | 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /imu_filter_madgwick/src/imu_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | #include 25 | 26 | #include "imu_filter_madgwick/imu_filter_ros.h" 27 | 28 | int main(int argc, char *argv[]) 29 | { 30 | rclcpp::init(argc, argv); 31 | 32 | rclcpp::executors::SingleThreadedExecutor exec; 33 | rclcpp::NodeOptions options; 34 | 35 | auto node = std::make_shared(options); 36 | exec.add_node(node); 37 | exec.spin(); 38 | 39 | rclcpp::shutdown(); 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /imu_filter_madgwick/launch/imu_filter_component.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | import launch_ros.actions 4 | import launch.substitutions 5 | import yaml 6 | 7 | from launch_ros.actions import ComposableNodeContainer 8 | from launch_ros.descriptions import ComposableNode 9 | from ament_index_python.packages import get_package_share_directory 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | param_config = os.path.join(get_package_share_directory('imu_filter_madgwick'), 'config', 'imu_filter.yaml') 15 | 16 | # https://github.com/ros2/rclcpp/issues/715#issuecomment-490425249 17 | # Composable Nodes use different yaml parsing than a standalone node. 18 | # This code will load the parameters from the yaml (removing the namespace/nodename/ros__parameters heading) so 19 | # that the parameters are parsed and named properly for the composable node. 20 | with open(param_config, 'r') as f: 21 | params = yaml.safe_load(f)['imu_filter']['ros__parameters'] 22 | 23 | container = ComposableNodeContainer( 24 | name='imu_filter_container', 25 | namespace='', 26 | package='rclcpp_components', 27 | executable='component_container', 28 | composable_node_descriptions=[ 29 | ComposableNode( 30 | package='imu_filter_madgwick', 31 | plugin='ImuFilterMadgwickRos', 32 | name='imu_filter', 33 | parameters=[params], 34 | ) 35 | ], 36 | output='screen', 37 | ) 38 | 39 | return launch.LaunchDescription([container]) 40 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | repos: 16 | # Standard hooks 17 | - repo: https://github.com/pre-commit/pre-commit-hooks 18 | rev: v4.1.0 19 | hooks: 20 | - id: check-added-large-files 21 | - id: check-case-conflict 22 | - id: check-executables-have-shebangs 23 | - id: check-docstring-first 24 | - id: check-merge-conflict 25 | - id: check-shebang-scripts-are-executable 26 | - id: check-symlinks 27 | - id: check-vcs-permalinks 28 | - id: check-xml 29 | - id: check-yaml 30 | - id: debug-statements 31 | - id: end-of-file-fixer 32 | exclude: &excludes | 33 | (?x)^( 34 | .*\.blend| 35 | .*\.dae| 36 | .*\.mtl| 37 | .*\.obj| 38 | .*\.pgm| 39 | .*\.step| 40 | .*\.stl| 41 | .*\.patch 42 | )$ 43 | - id: fix-byte-order-marker 44 | - id: mixed-line-ending 45 | exclude: *excludes 46 | - id: trailing-whitespace 47 | exclude: *excludes 48 | 49 | - repo: https://github.com/psf/black 50 | rev: 22.3.0 51 | hooks: 52 | - id: black 53 | 54 | - repo: local 55 | hooks: 56 | - id: clang-format 57 | name: clang-format 58 | description: Format files with ClangFormat. 59 | entry: clang-format 60 | language: system 61 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 62 | args: [ "-fallback-style=none", "-i" ] 63 | -------------------------------------------------------------------------------- /LICENSE.bsd: -------------------------------------------------------------------------------- 1 | Copyright (c) 2021, DFKI GmbH 2 | Copyright (c) 2015, City University of New York 3 | Copyright (c) 2012, Willow Garage, Inc. 4 | Copyright (c) 2012, Ivan Dryanovski 5 | Copyright (c) 2010, CCNY Robotics Lab 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | 11 | * Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | * Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /.github/workflows/github-actions.yml: -------------------------------------------------------------------------------- 1 | name: Build and run ROS tests 2 | on: 3 | push: 4 | pull_request: 5 | workflow_dispatch: 6 | inputs: 7 | debug_enabled: 8 | type: boolean 9 | description: 'Run the build with tmate debugging enabled (https://github.com/marketplace/actions/debugging-with-tmate)' 10 | required: false 11 | default: false 12 | jobs: 13 | build: 14 | strategy: 15 | matrix: 16 | rosdistro: [rolling] 17 | fail-fast: false 18 | runs-on: ubuntu-latest 19 | container: 20 | image: ros:${{ matrix.rosdistro }}-ros-core 21 | steps: 22 | # Enable tmate debugging of manually-triggered workflows if the input option was provided 23 | - name: Setup tmate session 24 | uses: mxschmitt/action-tmate@v3 25 | if: ${{ github.event_name == 'workflow_dispatch' && inputs.debug_enabled }} 26 | with: 27 | detached: true 28 | - name: Install apt dependencies 29 | run: | 30 | apt-get update 31 | apt-get install -y build-essential clang-format file git python3-pip python3-colcon-common-extensions python3-rosdep pre-commit 32 | - name: Checkout repository 33 | uses: actions/checkout@v4 34 | with: 35 | path: src/imu_tools 36 | - name: Use rosdep to install remaining dependencies 37 | run: | 38 | rosdep init 39 | rosdep update 40 | rosdep install --from-paths src -i -y --rosdistro ${{ matrix.rosdistro }} 41 | - name: Build 42 | run: | 43 | . /opt/ros/${{ matrix.rosdistro }}/setup.sh 44 | colcon build --parallel-workers 1 45 | - name: Run tests 46 | run: | 47 | . install/setup.sh 48 | colcon test --parallel-workers 1 49 | colcon test-result 50 | - name: Run pre-commit hooks 51 | run: | 52 | cd src/imu_tools 53 | pre-commit run -a 54 | -------------------------------------------------------------------------------- /imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 26 | #define IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | class StatelessOrientation 33 | { 34 | public: 35 | static bool computeOrientation(WorldFrame::WorldFrame frame, 36 | geometry_msgs::msg::Vector3 acceleration, 37 | geometry_msgs::msg::Vector3 magneticField, 38 | geometry_msgs::msg::Quaternion& orientation); 39 | 40 | static bool computeOrientation(WorldFrame::WorldFrame frame, 41 | geometry_msgs::msg::Vector3 acceleration, 42 | geometry_msgs::msg::Quaternion& orientation); 43 | }; 44 | 45 | #endif // IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 46 | -------------------------------------------------------------------------------- /imu_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package imu_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.1 (2025-05-15) 6 | ------------------ 7 | 8 | 2.2.0 (2024-10-01) 9 | ------------------ 10 | 11 | 2.1.4 (2024-04-26) 12 | ------------------ 13 | 14 | 2.1.3 (2022-12-07) 15 | ------------------ 16 | 17 | 2.1.2 (2022-07-14) 18 | ------------------ 19 | 20 | 2.1.1 (2022-05-24) 21 | ------------------ 22 | 23 | 2.1.0 (2022-05-02) 24 | ------------------ 25 | 26 | 2.0.0 (2022-04-12) 27 | ------------------ 28 | * Initial release into ROS2 foxy, galactic and rolling 29 | * Contributors: Martin Günther, tgreier 30 | 31 | 1.2.2 (2020-05-25) 32 | ------------------ 33 | 34 | 1.2.1 (2019-05-06) 35 | ------------------ 36 | 37 | 1.2.0 (2018-05-25) 38 | ------------------ 39 | 40 | 1.1.5 (2017-05-24) 41 | ------------------ 42 | 43 | 1.1.4 (2017-05-22) 44 | ------------------ 45 | 46 | 1.1.3 (2017-03-10) 47 | ------------------ 48 | 49 | 1.1.2 (2016-09-07) 50 | ------------------ 51 | 52 | 1.1.1 (2016-09-07) 53 | ------------------ 54 | 55 | 1.1.0 (2016-04-25) 56 | ------------------ 57 | 58 | 1.0.11 (2016-04-22) 59 | ------------------- 60 | 61 | 1.0.10 (2016-04-22) 62 | ------------------- 63 | 64 | 1.0.9 (2015-10-16) 65 | ------------------ 66 | 67 | 1.0.8 (2015-10-07) 68 | ------------------ 69 | * Add imu_complementary_filter to meta package 70 | * Contributors: Martin Günther 71 | 72 | 1.0.7 (2015-10-07) 73 | ------------------ 74 | 75 | 1.0.6 (2015-10-06) 76 | ------------------ 77 | 78 | 1.0.5 (2015-06-24) 79 | ------------------ 80 | 81 | 1.0.4 (2015-05-06) 82 | ------------------ 83 | 84 | 1.0.3 (2015-01-29) 85 | ------------------ 86 | 87 | 1.0.2 (2015-01-27) 88 | ------------------ 89 | 90 | 1.0.1 (2014-12-10) 91 | ------------------ 92 | * add me as maintainer to package.xml 93 | * Contributors: Martin Günther 94 | 95 | 1.0.0 (2014-09-03) 96 | ------------------ 97 | * First public release 98 | * catkinization of imu_tools metapackage 99 | * Contributors: Francisco Vina 100 | -------------------------------------------------------------------------------- /imu_complementary_filter/src/complementary_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "imu_complementary_filter/complementary_filter_ros.h" 35 | 36 | int main(int argc, char **argv) 37 | { 38 | rclcpp::init(argc, argv); 39 | auto filter = std::make_shared(); 40 | rclcpp::spin(filter); 41 | rclcpp::shutdown(); 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /imu_filter_madgwick/include/imu_filter_madgwick/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef IMU_FILTER_MADGWICK_CPP__VISIBILITY_CONTROL_H_ 16 | #define IMU_FILTER_MADGWICK_CPP__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" { 20 | #endif 21 | 22 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 23 | // https://gcc.gnu.org/wiki/Visibility 24 | 25 | #if defined _WIN32 || defined __CYGWIN__ 26 | #ifdef __GNUC__ 27 | #define IMU_FILTER_MADGWICK_CPP_EXPORT __attribute__((dllexport)) 28 | #define IMU_FILTER_MADGWICK_CPP_IMPORT __attribute__((dllimport)) 29 | #else 30 | #define IMU_FILTER_MADGWICK_CPP_EXPORT __declspec(dllexport) 31 | #define IMU_FILTER_MADGWICK_CPP_IMPORT __declspec(dllimport) 32 | #endif 33 | #ifdef IMU_FILTER_MADGWICK_CPP_BUILDING_DLL 34 | #define IMU_FILTER_MADGWICK_CPP_PUBLIC IMU_FILTER_MADGWICK_CPP_EXPORT 35 | #else 36 | #define IMU_FILTER_MADGWICK_CPP_PUBLIC IMU_FILTER_MADGWICK_CPP_IMPORT 37 | #endif 38 | #define IMU_FILTER_MADGWICK_CPP_PUBLIC_TYPE IMU_FILTER_MADGWICK_CPP_PUBLIC 39 | #define IMU_FILTER_MADGWICK_CPP_LOCAL 40 | #else 41 | #define IMU_FILTER_MADGWICK_CPP_EXPORT __attribute__((visibility("default"))) 42 | #define IMU_FILTER_MADGWICK_CPP_IMPORT 43 | #if __GNUC__ >= 4 44 | #define IMU_FILTER_MADGWICK_CPP_PUBLIC __attribute__((visibility("default"))) 45 | #define IMU_FILTER_MADGWICK_CPP_LOCAL __attribute__((visibility("hidden"))) 46 | #else 47 | #define IMU_FILTER_MADGWICK_CPP_PUBLIC 48 | #define IMU_FILTER_MADGWICK_CPP_LOCAL 49 | #endif 50 | #define IMU_FILTER_MADGWICK_CPP_PUBLIC_TYPE 51 | #endif 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif // IMU_FILTER_MADGWICK_CPP__VISIBILITY_CONTROL_H_ 58 | -------------------------------------------------------------------------------- /imu_filter_madgwick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(imu_filter_madgwick) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(rclcpp_components REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(geometry_msgs REQUIRED) 13 | find_package(tf2_geometry_msgs REQUIRED) 14 | find_package(tf2_ros REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | 17 | add_library(imu_filter_madgwick SHARED 18 | src/imu_filter.cpp 19 | src/imu_filter_ros.cpp 20 | src/stateless_orientation.cpp) 21 | target_compile_features(imu_filter_madgwick PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 22 | target_include_directories(imu_filter_madgwick PUBLIC 23 | $ 24 | $) 25 | target_link_libraries(imu_filter_madgwick PUBLIC 26 | ${geometry_msgs_TARGETS} 27 | ${sensor_msgs_TARGETS} 28 | ${std_msgs_TARGETS} 29 | rclcpp::rclcpp 30 | rclcpp_components::component 31 | rclcpp_components::component_manager 32 | sensor_msgs::sensor_msgs_library 33 | tf2_geometry_msgs::tf2_geometry_msgs 34 | tf2_ros::tf2_ros 35 | ) 36 | rclcpp_components_register_nodes(imu_filter_madgwick "ImuFilterMadgwickRos") 37 | target_compile_definitions(imu_filter_madgwick 38 | PRIVATE "IMU_FILTER_MADGWICK_CPP_BUILDING_DLL") 39 | 40 | add_executable(imu_filter_madgwick_node src/imu_filter_node.cpp) 41 | target_include_directories(imu_filter_madgwick_node PUBLIC 42 | $ 43 | $) 44 | target_link_libraries(imu_filter_madgwick_node imu_filter_madgwick) 45 | 46 | install( 47 | TARGETS imu_filter_madgwick 48 | EXPORT export_${PROJECT_NAME} 49 | ARCHIVE DESTINATION lib 50 | LIBRARY DESTINATION lib 51 | RUNTIME DESTINATION bin 52 | ) 53 | 54 | install( 55 | DIRECTORY include/ 56 | DESTINATION include 57 | ) 58 | 59 | install(TARGETS 60 | imu_filter_madgwick_node 61 | DESTINATION lib/${PROJECT_NAME}) 62 | 63 | install(DIRECTORY 64 | launch config 65 | DESTINATION share/${PROJECT_NAME}) 66 | 67 | if(BUILD_TESTING) 68 | find_package(ament_cmake_gtest REQUIRED) 69 | add_subdirectory(test) 70 | endif() 71 | 72 | ament_export_include_directories(include) 73 | ament_export_libraries(imu_filter_madgwick) 74 | ament_export_targets( 75 | export_${PROJECT_NAME} 76 | ) 77 | 78 | ament_package() 79 | -------------------------------------------------------------------------------- /imu_complementary_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(imu_complementary_filter) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(geometry_msgs REQUIRED) 10 | find_package(message_filters REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(std_msgs REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | find_package(tf2_ros REQUIRED) 16 | 17 | add_library(complementary_filter SHARED 18 | src/complementary_filter.cpp 19 | src/complementary_filter_ros.cpp 20 | include/imu_complementary_filter/complementary_filter.h 21 | include/imu_complementary_filter/complementary_filter_ros.h 22 | ) 23 | target_compile_features(complementary_filter PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 24 | target_include_directories(complementary_filter PUBLIC 25 | $ 26 | $) 27 | target_link_libraries(complementary_filter PUBLIC 28 | ${geometry_msgs_TARGETS} 29 | ${sensor_msgs_TARGETS} 30 | ${std_msgs_TARGETS} 31 | message_filters::message_filters 32 | rclcpp::rclcpp 33 | sensor_msgs::sensor_msgs_library 34 | tf2::tf2 35 | tf2_ros::tf2_ros 36 | ) 37 | # Causes the visibility macros to use dllexport rather than dllimport, 38 | # which is appropriate when building the dll but not consuming it. 39 | target_compile_definitions(complementary_filter PRIVATE "IMU_COMPLEMENTARY_FILTER_BUILDING_LIBRARY") 40 | 41 | # create complementary_filter_node executable 42 | add_executable(complementary_filter_node 43 | src/complementary_filter_node.cpp) 44 | target_include_directories(complementary_filter_node PUBLIC 45 | $ 46 | $) 47 | target_link_libraries(complementary_filter_node complementary_filter) 48 | 49 | install( 50 | TARGETS complementary_filter 51 | EXPORT export_${PROJECT_NAME} 52 | ARCHIVE DESTINATION lib 53 | LIBRARY DESTINATION lib 54 | RUNTIME DESTINATION bin 55 | ) 56 | install(TARGETS 57 | complementary_filter_node 58 | DESTINATION lib/${PROJECT_NAME} 59 | ) 60 | 61 | ## Mark cpp header files for installation 62 | install( 63 | DIRECTORY include/ 64 | DESTINATION include 65 | ) 66 | 67 | install(DIRECTORY launch config 68 | DESTINATION share/${PROJECT_NAME} 69 | ) 70 | 71 | ament_export_include_directories(include) 72 | ament_export_libraries(complementary_filter) 73 | ament_export_targets( 74 | export_${PROJECT_NAME} 75 | ) 76 | ament_package() 77 | -------------------------------------------------------------------------------- /rviz_imu_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(rviz_imu_plugin) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED COMPONENTS rviz) 14 | find_package(message_filters REQUIRED) 15 | find_package(Qt5 REQUIRED COMPONENTS Widgets Test) 16 | find_package(rviz_common REQUIRED) 17 | find_package(rviz_ogre_vendor REQUIRED) 18 | find_package(rviz_rendering REQUIRED) 19 | find_package(sensor_msgs REQUIRED) 20 | find_package(tf2 REQUIRED) 21 | find_package(tf2_ros REQUIRED) 22 | 23 | 24 | ## This setting causes Qt's "MOC" generation to happen automatically. 25 | set(CMAKE_AUTOMOC ON) 26 | 27 | 28 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 29 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 30 | add_definitions(-DQT_NO_KEYWORDS) 31 | 32 | 33 | include_directories(include) 34 | 35 | set(dependencies 36 | message_filters 37 | Qt5 38 | rviz_common 39 | rviz_ogre_vendor 40 | rviz_rendering 41 | sensor_msgs 42 | tf2 43 | tf2_ros 44 | ) 45 | 46 | ## Here we specify the list of source files. 47 | ## The generated MOC files are included automatically as headers. 48 | set(SRC_FILES src/imu_display.cpp 49 | src/imu_orientation_visual.cpp 50 | src/imu_axes_visual.cpp 51 | src/imu_acc_visual.cpp 52 | src/mag_display.cpp 53 | src/mag_visual.cpp) 54 | 55 | ## Build libraries 56 | add_library(${PROJECT_NAME} SHARED ${SRC_FILES}) 57 | 58 | #set_target_properties(${PROJECT_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON) 59 | 60 | ## Link the library with whatever Qt libraries have been defined by 61 | ## the ``find_package(Qt4 ...)`` line above, or by the 62 | ## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries 63 | ## catkin has included. 64 | 65 | target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor::OgreMain rviz_ogre_vendor::OgreOverlay Qt5::Widgets ${sensor_msgs_TARGETS} message_filters::message_filters rviz_common::rviz_common rviz_ogre_vendor::OgreMain rviz_rendering::rviz_rendering sensor_msgs::sensor_msgs_library tf2::tf2 tf2_ros::tf2_ros) 66 | 67 | # prevent pluginlib from using boost 68 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 69 | 70 | # must place the description in rviz_common package in order to have plugin recognized. 71 | pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) 72 | 73 | ament_export_include_directories(include) 74 | ament_export_libraries(${PROJECT_NAME}) 75 | ament_export_dependencies( 76 | Qt5 77 | rviz_common 78 | sensor_msgs 79 | tf2 80 | tf2_ros 81 | ) 82 | 83 | install(TARGETS ${PROJECT_NAME} 84 | EXPORT ${PROJECT_NAME} 85 | ARCHIVE DESTINATION lib 86 | LIBRARY DESTINATION lib 87 | RUNTIME DESTINATION bin 88 | ) 89 | 90 | ament_package() 91 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | IMU tools for ROS 2 | ================= 3 | 4 | Overview 5 | -------- 6 | 7 | IMU-related filters and visualizers. The repository contains: 8 | 9 | * `imu_filter_madgwick`: a filter which fuses angular velocities, 10 | accelerations, and (optionally) magnetic readings from a generic IMU 11 | device into an orientation. Based on the work of [1]. 12 | 13 | * `imu_complementary_filter`: a filter which fuses angular velocities, 14 | accelerations, and (optionally) magnetic readings from a generic IMU 15 | device into an orientation quaternion using a novel approach based on a 16 | complementary fusion. Based on the work of [2]. 17 | 18 | * `rviz_imu_plugin` a plugin for rviz which displays `sensor_msgs::Imu` messages 19 | 20 | [1]: https://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 21 | 22 | [2]: https://www.mdpi.com/1424-8220/15/8/19302 23 | 24 | 25 | Installing 26 | ---------- 27 | 28 | ### From binaries 29 | 30 | This repo has been released into all current ROS1 and ROS2 distros. To install, 31 | simply: 32 | 33 | sudo apt-get install ros--imu-tools 34 | 35 | ### From source (ROS1) 36 | 37 | [Create a catkin workspace](https://wiki.ros.org/catkin/Tutorials/create_a_workspace) 38 | (e.g., `~/catkin_ws/`) and source the `devel/setup.bash` file. 39 | 40 | Make sure you have git installed: 41 | 42 | sudo apt-get install git 43 | 44 | Clone this repository into your catkin workspace (e.g., `~/catin_ws/src`; use 45 | the proper branch for your distro, e.g., `melodic`, `noetic`, ...): 46 | 47 | git clone -b https://github.com/CCNYRoboticsLab/imu_tools.git 48 | 49 | Install any dependencies using [rosdep](https://www.ros.org/wiki/rosdep). 50 | 51 | rosdep install imu_tools 52 | 53 | Compile the stack: 54 | 55 | cd ~/catkin_ws 56 | catkin_make 57 | 58 | ### From source (ROS2) 59 | 60 | Follow the steps from the ROS2 [Creating a 61 | workspace](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html) 62 | documentation, but instead of cloning the sample repo, clone the proper branch 63 | of this repo instead: 64 | 65 | git clone -b https://github.com/CCNYRoboticsLab/imu_tools.git 66 | 67 | 68 | More info 69 | --------- 70 | 71 | All nodes, topics and parameters are documented on [this repo's ROS wiki 72 | page](https://wiki.ros.org/imu_tools). 73 | 74 | 75 | pre-commit formatting checks 76 | ---------------------------- 77 | 78 | This repo has a [pre-commit](https://pre-commit.com/) check that runs in CI. 79 | You can use this locally and set it up to run automatically before you commit 80 | something. To install, use apt: 81 | 82 | ```bash 83 | sudo apt install pre-commit 84 | ``` 85 | 86 | To run over all the files in the repo manually: 87 | 88 | ```bash 89 | pre-commit run -a 90 | ``` 91 | 92 | To run pre-commit automatically before committing in the local repo, install the git hooks: 93 | 94 | ```bash 95 | pre-commit install 96 | ``` 97 | 98 | License 99 | ------- 100 | 101 | * `imu_filter_madgwick`: currently licensed as GPL, following the original implementation 102 | 103 | * `imu_complementary_filter`: BSD 104 | 105 | * `rviz_imu_plugin`: BSD 106 | -------------------------------------------------------------------------------- /imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADWICK_IMU_FILTER_H 26 | #define IMU_FILTER_MADWICK_IMU_FILTER_H 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | class ImuFilter 33 | { 34 | public: 35 | ImuFilter(); 36 | virtual ~ImuFilter(); 37 | 38 | private: 39 | // **** paramaters 40 | double gain_; // algorithm gain 41 | double zeta_; // gyro drift bias gain 42 | WorldFrame::WorldFrame world_frame_; // NWU, ENU, NED 43 | 44 | // **** state variables 45 | double q0, q1, q2, q3; // quaternion 46 | float w_bx_, w_by_, w_bz_; // 47 | 48 | public: 49 | void setAlgorithmGain(double gain) 50 | { 51 | gain_ = gain; 52 | } 53 | 54 | void setDriftBiasGain(double zeta) 55 | { 56 | zeta_ = zeta; 57 | } 58 | 59 | void setWorldFrame(WorldFrame::WorldFrame frame) 60 | { 61 | world_frame_ = frame; 62 | } 63 | 64 | void getOrientation(double& q0, double& q1, double& q2, double& q3) 65 | { 66 | q0 = this->q0; 67 | q1 = this->q1; 68 | q2 = this->q2; 69 | q3 = this->q3; 70 | 71 | // perform precise normalization of the output, using 1/sqrt() 72 | // instead of the fast invSqrt() approximation. Without this, 73 | // TF2 complains that the quaternion is not normalized. 74 | double recipNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 75 | q0 *= recipNorm; 76 | q1 *= recipNorm; 77 | q2 *= recipNorm; 78 | q3 *= recipNorm; 79 | } 80 | 81 | void setOrientation(double q0, double q1, double q2, double q3) 82 | { 83 | this->q0 = q0; 84 | this->q1 = q1; 85 | this->q2 = q2; 86 | this->q3 = q3; 87 | 88 | w_bx_ = 0; 89 | w_by_ = 0; 90 | w_bz_ = 0; 91 | } 92 | 93 | void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, 94 | float az, float mx, float my, float mz, float dt); 95 | 96 | void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, 97 | float az, float dt); 98 | 99 | void getGravity(float& rx, float& ry, float& rz, float gravity = 9.80665); 100 | }; 101 | 102 | #endif // IMU_FILTER_MADWICK_IMU_FILTER_H 103 | -------------------------------------------------------------------------------- /imu_filter_madgwick/include/imu_filter_madgwick/base_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace imu_filter { 6 | 7 | class BaseNode : public rclcpp::Node 8 | { 9 | public: 10 | explicit BaseNode(std::string name, const rclcpp::NodeOptions &options) 11 | : Node(name, options) 12 | { 13 | } 14 | 15 | typedef struct { 16 | double from_value; 17 | double to_value; 18 | double step; 19 | } floating_point_range; 20 | 21 | typedef struct { 22 | int from_value; 23 | int to_value; 24 | int step; 25 | } integer_range; 26 | 27 | // Declare a parameter that has no integer or floating point range 28 | // constraints 29 | void add_parameter(const std::string &name, 30 | const rclcpp::ParameterValue &default_value, 31 | const std::string &description = "", 32 | const std::string &additional_constraints = "", 33 | bool read_only = false) 34 | { 35 | auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); 36 | 37 | descriptor.name = name; 38 | descriptor.description = description; 39 | descriptor.additional_constraints = additional_constraints; 40 | descriptor.read_only = read_only; 41 | 42 | declare_parameter(descriptor.name, default_value, descriptor); 43 | } 44 | 45 | // Declare a parameter that has a floating point range constraint 46 | void add_parameter(const std::string &name, 47 | const rclcpp::ParameterValue &default_value, 48 | const floating_point_range fp_range, 49 | const std::string &description = "", 50 | const std::string &additional_constraints = "", 51 | bool read_only = false) 52 | { 53 | auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); 54 | 55 | descriptor.name = name; 56 | descriptor.description = description; 57 | descriptor.additional_constraints = additional_constraints; 58 | descriptor.read_only = read_only; 59 | descriptor.floating_point_range.resize(1); 60 | descriptor.floating_point_range[0].from_value = fp_range.from_value; 61 | descriptor.floating_point_range[0].to_value = fp_range.to_value; 62 | descriptor.floating_point_range[0].step = fp_range.step; 63 | 64 | declare_parameter(descriptor.name, default_value, descriptor); 65 | } 66 | 67 | // Declare a parameter that has an integer range constraint 68 | void add_parameter(const std::string &name, 69 | const rclcpp::ParameterValue &default_value, 70 | const integer_range int_range, 71 | const std::string &description = "", 72 | const std::string &additional_constraints = "", 73 | bool read_only = false) 74 | { 75 | auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); 76 | 77 | descriptor.name = name; 78 | descriptor.description = description; 79 | descriptor.additional_constraints = additional_constraints; 80 | descriptor.read_only = read_only; 81 | descriptor.integer_range.resize(1); 82 | descriptor.integer_range[0].from_value = int_range.from_value; 83 | descriptor.integer_range[0].to_value = int_range.to_value; 84 | descriptor.integer_range[0].step = int_range.step; 85 | 86 | declare_parameter(descriptor.name, default_value, descriptor); 87 | } 88 | }; 89 | 90 | } // namespace imu_filter 91 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_axes_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H 32 | #define RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H 33 | 34 | #include 35 | #include 36 | 37 | namespace rviz_rendering { 38 | class Axes; 39 | } 40 | 41 | namespace rviz_imu_plugin { 42 | 43 | class ImuAxesVisual 44 | { 45 | public: 46 | // Constructor. Creates the visual stuff and puts it into the 47 | // scene, but in an unconfigured state. 48 | ImuAxesVisual(Ogre::SceneManager* scene_manager, 49 | Ogre::SceneNode* parent_node); 50 | 51 | // Destructor. Removes the visual stuff from the scene. 52 | virtual ~ImuAxesVisual(); 53 | 54 | // Configure the visual to show the data in the message. 55 | void setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg); 56 | 57 | // Set the pose of the coordinate frame the message refers to. 58 | // These could be done inside setMessage(), but that would require 59 | // calls to FrameManager and error handling inside setMessage(), 60 | // which doesn't seem as clean. This way ImuVisual is only 61 | // responsible for visualization. 62 | void setFramePosition(const Ogre::Vector3& position); 63 | void setFrameOrientation(const Ogre::Quaternion& orientation); 64 | 65 | // Set the color and alpha of the visual, which are user-editable 66 | 67 | void setScale(float scale); 68 | 69 | float getScale() 70 | { 71 | return scale_; 72 | } 73 | 74 | void show(); 75 | void hide(); 76 | 77 | private: 78 | inline bool checkQuaternionValidity( 79 | const sensor_msgs::msg::Imu::ConstSharedPtr msg); 80 | 81 | Ogre::Quaternion orientation_; 82 | 83 | float scale_; 84 | bool quat_valid_; 85 | 86 | rviz_rendering::Axes* orientation_axes_; 87 | 88 | // A SceneNode whose pose is set to match the coordinate frame of 89 | // the Imu message header. 90 | Ogre::SceneNode* frame_node_; 91 | 92 | // The SceneManager, kept here only so the destructor can ask it to 93 | // destroy the ``frame_node_``. 94 | Ogre::SceneManager* scene_manager_; 95 | }; 96 | 97 | } // namespace rviz_imu_plugin 98 | 99 | #endif // RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H 100 | -------------------------------------------------------------------------------- /rviz_imu_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_imu_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.1 (2025-05-15) 6 | ------------------ 7 | * Fix deprecated header includes (`#216 `_) 8 | * [kilted] Update deprecated call to ament_target_dependencies (`#215 `_) 9 | * Contributors: David V. Lu!!, Martin Günther 10 | 11 | 2.2.0 (2024-10-01) 12 | ------------------ 13 | 14 | 2.1.4 (2024-04-26) 15 | ------------------ 16 | 17 | 2.1.3 (2022-12-07) 18 | ------------------ 19 | 20 | 2.1.2 (2022-07-14) 21 | ------------------ 22 | 23 | 2.1.1 (2022-05-24) 24 | ------------------ 25 | 26 | 2.1.0 (2022-05-02) 27 | ------------------ 28 | 29 | 2.0.0 (2022-04-12) 30 | ------------------ 31 | * Initial release into ROS2 foxy, galactic and rolling 32 | * Fix gcc warnings + clang-tidy suggestions 33 | * Fix CMakeLists 34 | * Reformat everything using clang-format 35 | * rviz_imu_plugin: Fix include paths 36 | * rviz_imu_plugin: Use C++14 37 | * Fix package.xml dependencies 38 | * Port rviz plugin to ROS2, add new plugin (`#125 `_) 39 | * Add MagneticField plugin 40 | * Update imu rviz plugin to ROS2 41 | * Update maintainers in package.xml 42 | * Contributors: Martin Günther, tgreier 43 | 44 | 1.2.2 (2020-05-25) 45 | ------------------ 46 | * Export symbols so plugin can load 47 | * properly show/hide visualization when enabled/disabled 48 | * Contributors: CCNY Robotics Lab, Lou Amadio, Martin Günther, v4hn 49 | 50 | 1.2.1 (2019-05-06) 51 | ------------------ 52 | * Fix includes, typos and log messages 53 | * print ros_warn and give unit quaternion to ogre to prevent rviz crash (`#90 `_) 54 | * Contributors: Jackey-Huo, Martin Günther 55 | 56 | 1.2.0 (2018-05-25) 57 | ------------------ 58 | 59 | 1.1.5 (2017-05-24) 60 | ------------------ 61 | 62 | 1.1.4 (2017-05-22) 63 | ------------------ 64 | * Add option to display orientation in world frame (`#69 `_) 65 | Per REP 145 IMU orientation is in the world frame. Rotating the 66 | orientation data to transform into the sensor frame results in strange 67 | behavior, such as double-rotation of orientation on a robot. Provide an 68 | option to display orientation in the world frame, and enable it by 69 | default. Continue to translate the position of the data to the sensor 70 | frame. 71 | * Contributors: C. Andy Martin 72 | 73 | 1.1.3 (2017-03-10) 74 | ------------------ 75 | 76 | 1.1.2 (2016-09-07) 77 | ------------------ 78 | 79 | 1.1.1 (2016-09-07) 80 | ------------------ 81 | 82 | 1.1.0 (2016-04-25) 83 | ------------------ 84 | * Add qt5 dependencies to rviz_imu_plugin package.xml 85 | This fixes the compilation errors on Kinetic for Debian Jessie. 86 | * Contributors: Martin Guenther 87 | 88 | 1.0.11 (2016-04-22) 89 | ------------------- 90 | 91 | 1.0.10 (2016-04-22) 92 | ------------------- 93 | * Support qt4/qt5 using rviz's exported qt version 94 | Closes `#58 `_ . 95 | This fixes the build on Kinetic, where only Qt5 is available, and 96 | is backwards compatible with Qt4 for Indigo. 97 | * Contributors: Martin Guenther 98 | 99 | 1.0.9 (2015-10-16) 100 | ------------------ 101 | 102 | 1.0.8 (2015-10-07) 103 | ------------------ 104 | 105 | 1.0.7 (2015-10-07) 106 | ------------------ 107 | 108 | 1.0.6 (2015-10-06) 109 | ------------------ 110 | 111 | 1.0.5 (2015-06-24) 112 | ------------------ 113 | 114 | 1.0.4 (2015-05-06) 115 | ------------------ 116 | 117 | 1.0.3 (2015-01-29) 118 | ------------------ 119 | 120 | 1.0.2 (2015-01-27) 121 | ------------------ 122 | 123 | 1.0.1 (2014-12-10) 124 | ------------------ 125 | * add me as maintainer to package.xml 126 | * Contributors: Martin Günther 127 | 128 | 1.0.0 (2014-09-03) 129 | ------------------ 130 | * First public release 131 | * Contributors: Ivan Dryanovski, Martin Günther, Davide Tateo, Francisco Vina, Lorenzo Riano 132 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/mag_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | #ifndef RVIZ_IMU_PLUGIN_MAG_VISUAL_H 31 | #define RVIZ_IMU_PLUGIN_MAG_VISUAL_H 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace rviz_rendering { 38 | class Arrow; 39 | } 40 | 41 | namespace rviz_imu_plugin { 42 | 43 | class MagVisual 44 | { 45 | public: 46 | // Constructor. Creates the visual stuff and puts it into the 47 | // scene, but in an unconfigured state. 48 | MagVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); 49 | 50 | // Destructor. Removes the visual stuff from the scene. 51 | virtual ~MagVisual(); 52 | 53 | // Configure the visual to show the data in the message. 54 | void setMessage(sensor_msgs::msg::MagneticField::ConstSharedPtr msg); 55 | 56 | // Set the pose of the coordinate frame the message refers to. 57 | // These could be done inside setMessage(), but that would require 58 | // calls to FrameManager and error handling inside setMessage(), 59 | // which doesn't seem as clean. This way ImuVisual is only 60 | // responsible for visualization. 61 | void setFramePosition(const Ogre::Vector3& position); 62 | void setFrameOrientation(const Ogre::Quaternion& orientation); 63 | 64 | // Set the color and alpha of the visual, which are user-editable 65 | 66 | void setScale(float scale); 67 | void setColor(const QColor& color); 68 | void setAlpha(float alpha); 69 | void set2d(bool twod) 70 | { 71 | is_2d_ = twod; 72 | } 73 | 74 | float getScale() 75 | { 76 | return scale_; 77 | } 78 | const QColor& getColor() 79 | { 80 | return color_; 81 | } 82 | float getAlpha() 83 | { 84 | return alpha_; 85 | } 86 | bool get2d() 87 | { 88 | return is_2d_; 89 | } 90 | 91 | void show(); 92 | void hide(); 93 | 94 | private: 95 | rviz_rendering::Arrow* heading_vector_; 96 | 97 | Ogre::Vector3 direction_; // computed from IMU message 98 | 99 | float arrow_length_; // computed from IMU message 100 | float arrow_radius_; 101 | float head_length_; 102 | float head_radius_; 103 | 104 | float scale_; 105 | float alpha_; 106 | QColor color_; 107 | bool is_2d_; 108 | 109 | // A SceneNode whose pose is set to match the coordinate frame of 110 | // the Imu message header. 111 | Ogre::SceneNode* frame_node_; 112 | 113 | // The SceneManager, kept here only so the destructor can ask it to 114 | // destroy the ``frame_node_``. 115 | Ogre::SceneManager* scene_manager_; 116 | }; 117 | 118 | } // namespace rviz_imu_plugin 119 | 120 | #endif // RVIZ_IMU_PLUGIN_MAG_VISUAL_H 121 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/mag_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef RVIZ_IMU_PLUGIN_MAG_DISPLAY_H 32 | #define RVIZ_IMU_PLUGIN_MAG_DISPLAY_H 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "mag_visual.h" 49 | 50 | namespace Ogre { 51 | class SceneNode; 52 | } 53 | 54 | namespace rviz_imu_plugin { 55 | 56 | class MagDisplay 57 | : public rviz_common::MessageFilterDisplay 58 | { 59 | Q_OBJECT 60 | public: 61 | MagDisplay(); 62 | ~MagDisplay() override; 63 | 64 | void onInitialize() override; 65 | void onEnable() override; 66 | void onDisable() override; 67 | 68 | void reset() override; 69 | 70 | void update(float dt, float ros_dt) override; 71 | 72 | private: 73 | void createProperties(); 74 | 75 | const std::string& getTopic() 76 | { 77 | return topic_; 78 | } 79 | 80 | float getMagScale() 81 | { 82 | return mag_visual_->getScale(); 83 | } 84 | float getMagAlpha() 85 | { 86 | return mag_visual_->getAlpha(); 87 | } 88 | const QColor& getMagColor() 89 | { 90 | return mag_visual_->getColor(); 91 | } 92 | 93 | protected Q_SLOTS: 94 | 95 | void updateMag(); 96 | 97 | private: 98 | // Property objects for user-editable properties. 99 | rviz_common::properties::BoolProperty* mag_2d_property_{}; 100 | rviz_common::properties::FloatProperty* mag_scale_property_{}; 101 | rviz_common::properties::ColorProperty* mag_color_property_{}; 102 | rviz_common::properties::FloatProperty* mag_alpha_property_{}; 103 | 104 | MagVisual* mag_visual_{}; 105 | 106 | // User-editable property variables. 107 | std::string topic_; 108 | bool mag_enabled_{}; 109 | bool twod_visual_; 110 | 111 | // A node in the Ogre scene tree to be the parent of all our visuals. 112 | Ogre::SceneNode* scene_node_; 113 | 114 | int messages_received_; 115 | 116 | // Function to handle an incoming ROS message. 117 | void processMessage( 118 | sensor_msgs::msg::MagneticField::ConstSharedPtr msg) override; 119 | }; 120 | 121 | } // namespace rviz_imu_plugin 122 | 123 | #endif // RVIZ_IMU_PLUGIN_MAG_DISPLAY_H 124 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_acc_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | #ifndef RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H 31 | #define RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace rviz_rendering { 38 | class Arrow; 39 | } 40 | 41 | namespace rviz_imu_plugin { 42 | 43 | class ImuAccVisual 44 | { 45 | public: 46 | // Constructor. Creates the visual stuff and puts it into the 47 | // scene, but in an unconfigured state. 48 | ImuAccVisual(Ogre::SceneManager* scene_manager, 49 | Ogre::SceneNode* parent_node); 50 | 51 | // Destructor. Removes the visual stuff from the scene. 52 | virtual ~ImuAccVisual(); 53 | 54 | // Configure the visual to show the data in the message. 55 | void setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg); 56 | 57 | // Set the pose of the coordinate frame the message refers to. 58 | // These could be done inside setMessage(), but that would require 59 | // calls to FrameManager and error handling inside setMessage(), 60 | // which doesn't seem as clean. This way ImuVisual is only 61 | // responsible for visualization. 62 | void setFramePosition(const Ogre::Vector3& position); 63 | void setFrameOrientation(const Ogre::Quaternion& orientation); 64 | 65 | // Set the color and alpha of the visual, which are user-editable 66 | 67 | void setScale(float scale); 68 | void setColor(const QColor& color); 69 | void setAlpha(float alpha); 70 | void setDerotated(bool derotated); 71 | 72 | float getScale() 73 | { 74 | return scale_; 75 | } 76 | const QColor& getColor() 77 | { 78 | return color_; 79 | } 80 | float getAlpha() 81 | { 82 | return alpha_; 83 | } 84 | bool getDerotated() 85 | { 86 | return derotated_; 87 | } 88 | 89 | void show(); 90 | void hide(); 91 | 92 | private: 93 | void create(); 94 | 95 | rviz_rendering::Arrow* acc_vector_; 96 | 97 | Ogre::Vector3 direction_; // computed from IMU message 98 | 99 | float arrow_length_; // computed from IMU message 100 | float arrow_radius_; 101 | float head_length_; 102 | float head_radius_; 103 | 104 | float scale_; 105 | float alpha_; 106 | QColor color_; 107 | 108 | bool derotated_; 109 | 110 | // A SceneNode whose pose is set to match the coordinate frame of 111 | // the Imu message header. 112 | Ogre::SceneNode* frame_node_; 113 | 114 | // The SceneManager, kept here only so the destructor can ask it to 115 | // destroy the ``frame_node_``. 116 | Ogre::SceneManager* scene_manager_; 117 | }; 118 | 119 | } // namespace rviz_imu_plugin 120 | 121 | #endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 122 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_orientation_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | #ifndef RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 31 | #define RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace rviz_rendering { 38 | class Shape; 39 | } 40 | 41 | namespace rviz_imu_plugin { 42 | 43 | class ImuOrientationVisual 44 | { 45 | public: 46 | // Constructor. Creates the visual stuff and puts it into the 47 | // scene, but in an unconfigured state. 48 | ImuOrientationVisual(Ogre::SceneManager* scene_manager, 49 | Ogre::SceneNode* parent_node); 50 | 51 | // Destructor. Removes the visual stuff from the scene. 52 | virtual ~ImuOrientationVisual(); 53 | 54 | // Configure the visual to show the data in the message. 55 | void setMessage(sensor_msgs::msg::Imu::ConstSharedPtr msg); 56 | 57 | // Set the pose of the coordinate frame the message refers to. 58 | // These could be done inside setMessage(), but that would require 59 | // calls to FrameManager and error handling inside setMessage(), 60 | // which doesn't seem as clean. This way ImuVisual is only 61 | // responsible for visualization. 62 | void setFramePosition(const Ogre::Vector3& position); 63 | void setFrameOrientation(const Ogre::Quaternion& orientation); 64 | 65 | // Set the color and alpha of the visual, which are user-editable 66 | 67 | void setScaleX(float x); 68 | void setScaleY(float y); 69 | void setScaleZ(float z); 70 | void setColor(const QColor& color); 71 | void setAlpha(float alpha); 72 | 73 | float getScaleX() 74 | { 75 | return scale_x_; 76 | } 77 | float getScaleY() 78 | { 79 | return scale_y_; 80 | } 81 | float getScaleZ() 82 | { 83 | return scale_z_; 84 | } 85 | const QColor& getColor() 86 | { 87 | return color_; 88 | } 89 | float getAlpha() 90 | { 91 | return alpha_; 92 | } 93 | 94 | void show(); 95 | void hide(); 96 | 97 | private: 98 | inline bool checkQuaternionValidity( 99 | sensor_msgs::msg::Imu::ConstSharedPtr msg); 100 | 101 | Ogre::Quaternion orientation_; 102 | 103 | float scale_x_, scale_y_, scale_z_; 104 | QColor color_; 105 | float alpha_; 106 | bool quat_valid_; 107 | 108 | rviz_rendering::Shape* orientation_box_; 109 | 110 | // A SceneNode whose pose is set to match the coordinate frame of 111 | // the Imu message header. 112 | Ogre::SceneNode* frame_node_; 113 | 114 | // The SceneManager, kept here only so the destructor can ask it to 115 | // destroy the ``frame_node_``. 116 | Ogre::SceneManager* scene_manager_; 117 | }; 118 | 119 | } // namespace rviz_imu_plugin 120 | 121 | #endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 122 | -------------------------------------------------------------------------------- /imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | #pragma once 25 | 26 | #include "rclcpp/rclcpp.hpp" 27 | #include "std_msgs/msg/string.hpp" 28 | 29 | #include "tf2_ros/transform_broadcaster.h" 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "imu_filter_madgwick/imu_filter.h" 39 | #include "imu_filter_madgwick/base_node.hpp" 40 | #include "imu_filter_madgwick/visibility_control.h" 41 | 42 | class ImuFilterMadgwickRos : public imu_filter::BaseNode 43 | { 44 | typedef sensor_msgs::msg::Imu ImuMsg; 45 | typedef sensor_msgs::msg::MagneticField MagMsg; 46 | typedef geometry_msgs::msg::Vector3Stamped RpyVectorMsg; 47 | 48 | typedef message_filters::sync_policies::ApproximateTime 49 | SyncPolicy; 50 | typedef message_filters::Synchronizer Synchronizer; 51 | typedef message_filters::Subscriber ImuSubscriber; 52 | typedef message_filters::Subscriber MagSubscriber; 53 | 54 | public: 55 | IMU_FILTER_MADGWICK_CPP_PUBLIC 56 | explicit ImuFilterMadgwickRos(const rclcpp::NodeOptions& options); 57 | 58 | // Callbacks are public so they can be called when used as a library 59 | void imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw); 60 | void imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, 61 | MagMsg::ConstSharedPtr mag_msg); 62 | 63 | private: 64 | std::shared_ptr imu_subscriber_; 65 | std::shared_ptr mag_subscriber_; 66 | std::shared_ptr sync_; 67 | 68 | rclcpp::Publisher::SharedPtr rpy_filtered_debug_publisher_; 69 | rclcpp::Publisher::SharedPtr rpy_raw_debug_publisher_; 70 | rclcpp::Publisher::SharedPtr imu_publisher_; 71 | tf2_ros::TransformBroadcaster tf_broadcaster_; 72 | 73 | rclcpp::TimerBase::SharedPtr check_topics_timer_; 74 | 75 | // Subscription for parameter change 76 | rclcpp::AsyncParametersClient::SharedPtr parameters_client_; 77 | rclcpp::Subscription::SharedPtr 78 | parameter_event_sub_; 79 | 80 | // **** paramaters 81 | WorldFrame::WorldFrame world_frame_; 82 | bool use_mag_{}; 83 | bool stateless_{}; 84 | bool publish_tf_{}; 85 | bool reverse_tf_{}; 86 | std::string fixed_frame_; 87 | std::string imu_frame_; 88 | double constant_dt_; 89 | bool publish_debug_topics_{}; 90 | bool remove_gravity_vector_{}; 91 | geometry_msgs::msg::Vector3 mag_bias_; 92 | double orientation_variance_; 93 | double yaw_offset_total_; 94 | 95 | // **** state variables 96 | std::mutex mutex_; 97 | bool initialized_; 98 | rclcpp::Time last_time_; 99 | tf2::Quaternion yaw_offsets_; 100 | 101 | // **** filter implementation 102 | ImuFilter filter_; 103 | 104 | // **** member functions 105 | void publishFilteredMsg(ImuMsg::ConstSharedPtr imu_msg_raw); 106 | void publishTransform(ImuMsg::ConstSharedPtr imu_msg_raw); 107 | 108 | void publishRawMsg(const rclcpp::Time& t, float roll, float pitch, 109 | float yaw); 110 | 111 | void reconfigCallback(rcl_interfaces::msg::ParameterEvent::SharedPtr event); 112 | void checkTopicsTimerCallback(); 113 | 114 | void applyYawOffset(double& q0, double& q1, double& q2, double& q3); 115 | }; 116 | -------------------------------------------------------------------------------- /imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 35 | #define IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "imu_complementary_filter/complementary_filter.h" 51 | 52 | namespace imu_tools { 53 | 54 | class ComplementaryFilterROS : public rclcpp::Node 55 | { 56 | public: 57 | ComplementaryFilterROS(); 58 | ~ComplementaryFilterROS() override; 59 | 60 | private: 61 | // Convenience typedefs 62 | typedef sensor_msgs::msg::Imu ImuMsg; 63 | typedef sensor_msgs::msg::MagneticField MagMsg; 64 | typedef geometry_msgs::msg::Vector3Stamped RpyVectorMsg; 65 | typedef message_filters::sync_policies::ApproximateTime 66 | SyncPolicy; 67 | typedef message_filters::Synchronizer Synchronizer; 68 | typedef message_filters::Subscriber ImuSubscriber; 69 | typedef message_filters::Subscriber MagSubscriber; 70 | 71 | // ROS-related variables. 72 | std::shared_ptr imu_subscriber_; 73 | std::shared_ptr mag_subscriber_; 74 | std::shared_ptr sync_; 75 | 76 | rclcpp::Publisher::SharedPtr imu_publisher_; 77 | rclcpp::Publisher::SharedPtr rpy_publisher_; 78 | rclcpp::Publisher::SharedPtr state_publisher_; 79 | tf2_ros::TransformBroadcaster tf_broadcaster_; 80 | 81 | // Parameters: 82 | bool use_mag_{}; 83 | bool publish_tf_{}; 84 | bool reverse_tf_{}; 85 | double constant_dt_{}; 86 | bool publish_debug_topics_{}; 87 | std::string fixed_frame_; 88 | double orientation_variance_{}; 89 | 90 | // State variables: 91 | ComplementaryFilter filter_; 92 | rclcpp::Time time_prev_; 93 | bool initialized_filter_; 94 | 95 | void initializeParams(); 96 | void imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw); 97 | void imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, 98 | MagMsg::ConstSharedPtr mav_msg); 99 | void publish(ImuMsg::ConstSharedPtr imu_msg_raw); 100 | 101 | tf2::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2, 102 | double q3) const; 103 | }; 104 | 105 | } // namespace imu_tools 106 | 107 | #endif // IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 108 | -------------------------------------------------------------------------------- /imu_filter_madgwick/test/test_helpers.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef TEST_TEST_HELPERS_H_ 3 | #define TEST_TEST_HELPERS_H_ 4 | 5 | #include 6 | #include 7 | 8 | #define MAX_DIFF 0.05 9 | 10 | template 11 | static inline void normalize_quaternion(T& q0, T& q1, T& q2, T& q3) 12 | { 13 | T invNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 14 | T max = q0; 15 | if (fabs(max) < fabs(q1)) max = q1; 16 | if (fabs(max) < fabs(q2)) max = q2; 17 | if (fabs(max) < fabs(q3)) max = q3; 18 | if (max < 0) invNorm *= -1.0; 19 | 20 | q0 *= invNorm; 21 | q1 *= invNorm; 22 | q2 *= invNorm; 23 | q3 *= invNorm; 24 | } 25 | 26 | // Tests for normalization in the same way as tf2: 27 | // https://github.com/ros/geometry2/blob/bd490515b1434caeff521ea14901dfe04063ca27/tf2/src/buffer_core.cpp#L244-L247 28 | template 29 | static inline bool is_normalized(T q0, T q1, T q2, T q3) 30 | { 31 | return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0f) < 10e-6; 32 | } 33 | 34 | template 35 | static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, 36 | T qr3) 37 | { 38 | normalize_quaternion(q0, q1, q2, q3); 39 | normalize_quaternion(qr0, qr1, qr2, qr3); 40 | 41 | return (fabs(q0 - qr0) < MAX_DIFF) && (fabs(q1 - qr1) < MAX_DIFF) && 42 | (fabs(q2 - qr2) < MAX_DIFF) && (fabs(q3 - qr3) < MAX_DIFF); 43 | } 44 | 45 | template 46 | static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, 47 | T qr3) 48 | { 49 | // assert q == qr * qz 50 | tf2::Quaternion q(q1, q2, q3, q0); 51 | tf2::Quaternion qr(qr1, qr2, qr3, qr0); 52 | tf2::Quaternion qz = q * qr.inverse(); 53 | 54 | // remove x and y components. 55 | qz.setX(0.0); 56 | qz.setY(0.0); 57 | 58 | tf2::Quaternion qr_ = qz * qr; 59 | 60 | return quat_equal(q0, q1, q2, q3, qr_.getW(), qr_.getX(), qr_.getY(), 61 | qr_.getZ()); 62 | } 63 | 64 | #define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) \ 65 | ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) \ 66 | << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 67 | #define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__) 68 | 69 | #define ASSERT_QUAT_EQUAL_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) \ 70 | ASSERT_TRUE(quat_equal(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) \ 71 | << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 72 | #define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__) 73 | 74 | #define ASSERT_QUAT_EQUAL_EX_Z_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) \ 75 | ASSERT_TRUE(quat_eq_ex_z(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) \ 76 | << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 77 | #define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__) 78 | 79 | // Well known states 80 | // scheme: AM_x_y_z 81 | #define AM_EAST_NORTH_UP /* Acceleration */ \ 82 | 0.0, 0.0, 9.81, /* Magnetic */ 0.0, 0.0005, -0.0005 83 | #define AM_NORTH_EAST_DOWN /* Acceleration */ \ 84 | 0.0, 0.0, -9.81, /* Magnetic */ 0.0005, 0.0, 0.0005 85 | #define AM_NORTH_WEST_UP /* Acceleration */ \ 86 | 0.0, 0.0, 9.81, /* Magnetic */ 0.0005, 0.0, -0.0005 87 | #define AM_SOUTH_UP_WEST /* Acceleration */ \ 88 | 0.0, 9.81, 0.0, /* Magnetic */ -0.0005, -0.0005, 0.0 89 | #define AM_SOUTH_EAST_UP /* Acceleration */ \ 90 | 0.0, 0.0, 9.81, /* Magnetic */ -0.0005, 0.0, -0.0005 91 | #define AM_WEST_NORTH_DOWN /* Acceleration */ \ 92 | 0.0, 0.0, -9.81, /* Magnetic */ 0.0, 0.0005, 0.0005 93 | 94 | // Real sensor data 95 | #define AM_WEST_NORTH_DOWN_RSD /* Acceleration */ \ 96 | 0.12, 0.29, -9.83, /* Magnetic */ 6.363e-06, 1.0908e-05, 4.2723e-05 97 | #define AM_NE_NW_UP_RSD /* Acceleration */ \ 98 | 0.20, 0.55, 9.779, /* Magnetic */ 8.484e-06, 8.181e-06, -4.3329e-05 99 | 100 | // Strip accelration from am 101 | #define ACCEL_ONLY(ax, ay, az, mx, my, mz) ax, ay, az 102 | 103 | // Well known quaternion 104 | // scheme: QUAT_axis_angle 105 | #define QUAT_IDENTITY 1.0, 0.0, 0.0, 0.0 106 | #define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107 107 | #define QUAT_X_180 0.0, 1.0, 0.0, 0.0 108 | #define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5 109 | 110 | #define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012 111 | /* axis: (0.864401, 0.502559, 0.0120614) | angle: 178.848deg */ 112 | 113 | #define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02 114 | /* Axis: (-0.2, -0.96, 0.02), Angle: 180deg */ 115 | 116 | #define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50 117 | /* Axis: -Z, Angle: 60deg */ 118 | 119 | #define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41 120 | /* axis: (0.0300376, -0.020025, -0.410513) | angle: 48.6734deg */ 121 | 122 | #define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35 123 | /* Axis: Z, Angle: 41deg */ 124 | 125 | #define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02 126 | /* Axis: (0.9, 0.4, 0.0), Angle: 180deg */ 127 | 128 | #endif /* TEST_TEST_HELPERS_H_ */ 129 | -------------------------------------------------------------------------------- /imu_complementary_filter/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package imu_complementary_filter 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.1 (2025-05-15) 6 | ------------------ 7 | * Fix deprecated header includes (`#216 `_) 8 | * [kilted] Update deprecated call to ament_target_dependencies (`#215 `_) 9 | * Contributors: David V. Lu!!, Martin Günther 10 | 11 | 2.2.0 (2024-10-01) 12 | ------------------ 13 | * Add QoS overriding options (`#207 `_) 14 | * Contributors: Aleksander Szymański 15 | 16 | 2.1.4 (2024-04-26) 17 | ------------------ 18 | * Set read-only parameters as read_only (`#185 `_) 19 | * Contributors: Christoph Fröhlich 20 | 21 | 2.1.3 (2022-12-07) 22 | ------------------ 23 | * complementary: Build shared library 24 | See `#172 `_. 25 | * Update CMakeLists to use targets 26 | * Remove node\_ prefix. (`#163 `_) 27 | * Contributors: Martin Günther, Max Polzin 28 | 29 | 2.1.2 (2022-07-14) 30 | ------------------ 31 | 32 | 2.1.1 (2022-05-24) 33 | ------------------ 34 | * Add missing build dependency to package.xml. (`#161 `_) 35 | * Contributors: Martin Günther, Steven! Ragnarök 36 | 37 | 2.1.0 (2022-05-02) 38 | ------------------ 39 | * complementary: Add missing dependency on geometry_msgs 40 | * Contributors: Martin Günther 41 | 42 | 2.0.0 (2022-04-12) 43 | ------------------ 44 | * Initial release into ROS2 foxy, galactic and rolling 45 | * Fix gcc warnings + clang-tidy suggestions 46 | * Fix CMakeLists 47 | * Reformat python code using black 48 | * Manually reformat licenses + defines 49 | * Reformat everything using clang-format 50 | * Fix trailing whitespace 51 | * Add launch directory to CMakeLists.txt (`#146 `_) 52 | * Port imu_complementary_filter to ROS2 (`#138 `_) 53 | * Madgwick for eloquent (`#110 `_) 54 | * Contributors: Guido Sanchez, Martin Günther, Maximilian Schik, tgreier 55 | 56 | 1.2.2 (2020-05-25) 57 | ------------------ 58 | * fix install path & boost linkage issues 59 | * Contributors: Martin Günther, Sean Yen 60 | 61 | 1.2.1 (2019-05-06) 62 | ------------------ 63 | * Remove junk xml (`#93 `_) 64 | * Fix C++14 builds (`#89 `_) 65 | * Contributors: David V. Lu!!, Paul Bovbel 66 | 67 | 1.2.0 (2018-05-25) 68 | ------------------ 69 | * Add std dev parameter to orientation estimate from filter (`#85 `_) 70 | Similar to `#41 `_, but not using dynamic_reconfigure as not implemented for complementary filter 71 | * Contributors: Stefan Kohlbrecher 72 | 73 | 1.1.5 (2017-05-24) 74 | ------------------ 75 | 76 | 1.1.4 (2017-05-22) 77 | ------------------ 78 | 79 | 1.1.3 (2017-03-10) 80 | ------------------ 81 | * complementary_filter: move const initializations out of header 82 | Initialization of static consts other than int (here: float) inside the 83 | class declaration is not permitted in C++. It works in gcc (due to a 84 | non-standard extension), but throws an error in C++11. 85 | * Contributors: Martin Guenther 86 | 87 | 1.1.2 (2016-09-07) 88 | ------------------ 89 | 90 | 1.1.1 (2016-09-07) 91 | ------------------ 92 | 93 | 1.1.0 (2016-04-25) 94 | ------------------ 95 | 96 | 1.0.11 (2016-04-22) 97 | ------------------- 98 | 99 | 1.0.10 (2016-04-22) 100 | ------------------- 101 | * Remove Eigen dependency 102 | Eigen is not actually used anywhere. Thanks @asimay! 103 | * Removed main function from shared library 104 | * Contributors: Martin Guenther, Matthias Nieuwenhuisen 105 | 106 | 1.0.9 (2015-10-16) 107 | ------------------ 108 | * complementary: Add Eigen dependency 109 | Fixes `#54 `_. 110 | * Contributors: Martin Günther 111 | 112 | 1.0.8 (2015-10-07) 113 | ------------------ 114 | 115 | 1.0.7 (2015-10-07) 116 | ------------------ 117 | * Allow remapping imu namespace 118 | * Publish RPY as Vector3Stamped 119 | * Add params: constant_dt, publish_tf, reverse_tf, publish_debug_topics 120 | * Use MagneticField instead of Vector3 121 | * Contributors: Martin Günther 122 | 123 | 1.0.6 (2015-10-06) 124 | ------------------ 125 | * Add new package: imu_complementary_filter 126 | * Contributors: Roberto G. Valentini, Martin Günther, Michael Görner 127 | 128 | 1.0.5 (2015-06-24) 129 | ------------------ 130 | 131 | 1.0.4 (2015-05-06) 132 | ------------------ 133 | 134 | 1.0.3 (2015-01-29) 135 | ------------------ 136 | 137 | 1.0.2 (2015-01-27) 138 | ------------------ 139 | 140 | 1.0.1 (2014-12-10) 141 | ------------------ 142 | 143 | 1.0.0 (2014-11-28) 144 | ------------------ 145 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/mag_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include 33 | 34 | #include "mag_visual.h" 35 | #include 36 | 37 | namespace rviz_imu_plugin { 38 | 39 | MagVisual::MagVisual(Ogre::SceneManager* scene_manager, 40 | Ogre::SceneNode* parent_node) 41 | : heading_vector_(nullptr), 42 | arrow_length_(2.0), 43 | arrow_radius_(0.10), 44 | head_length_(0.20), 45 | head_radius_(0.10), 46 | scale_(0.05), 47 | alpha_(1.0), 48 | color_(1.0, 1.0, 0.0), 49 | is_2d_(true) 50 | { 51 | scene_manager_ = scene_manager; 52 | 53 | // Ogre::SceneNode s form a tree, with each node storing the 54 | // transform (position and orientation) of itself relative to its 55 | // parent. Ogre does the math of combining those transforms when it 56 | // is time to render. 57 | // 58 | // Here we create a node to store the pose of the Imu's header frame 59 | // relative to the RViz fixed frame. 60 | frame_node_ = parent_node->createChildSceneNode(); 61 | } 62 | 63 | MagVisual::~MagVisual() 64 | { 65 | hide(); 66 | 67 | // Destroy the frame node since we don't need it anymore. 68 | scene_manager_->destroySceneNode(frame_node_); 69 | } 70 | 71 | void MagVisual::show() 72 | { 73 | if (!heading_vector_) 74 | { 75 | heading_vector_ = 76 | new rviz_rendering::Arrow(scene_manager_, frame_node_); 77 | heading_vector_->setColor(color_.redF(), color_.greenF(), 78 | color_.blueF(), alpha_); 79 | heading_vector_->setDirection(direction_); 80 | heading_vector_->set(arrow_length_ * scale_, arrow_radius_ * scale_, 81 | head_length_ * scale_, head_radius_ * scale_); 82 | } 83 | } 84 | 85 | void MagVisual::hide() 86 | { 87 | if (heading_vector_) 88 | { 89 | delete heading_vector_; 90 | heading_vector_ = nullptr; 91 | } 92 | } 93 | 94 | void MagVisual::setMessage( 95 | const sensor_msgs::msg::MagneticField::ConstSharedPtr msg) 96 | { 97 | if (is_2d_) 98 | { 99 | direction_ = Ogre::Vector3(msg->magnetic_field.x, msg->magnetic_field.y, 100 | 0.0); // msg->magnetic_field.z); 101 | } else 102 | { 103 | direction_ = Ogre::Vector3(msg->magnetic_field.x, msg->magnetic_field.y, 104 | msg->magnetic_field.z); 105 | } 106 | direction_.normalise(); 107 | direction_ *= arrow_length_; 108 | 109 | if (heading_vector_) 110 | { 111 | heading_vector_->setDirection(direction_); 112 | heading_vector_->set(arrow_length_ * scale_, arrow_radius_ * scale_, 113 | head_length_ * scale_, head_radius_ * scale_); 114 | } 115 | } 116 | 117 | void MagVisual::setScale(float scale) 118 | { 119 | scale_ = scale; 120 | if (heading_vector_) 121 | { 122 | heading_vector_->setDirection(direction_); 123 | heading_vector_->set(arrow_length_ * scale_, arrow_radius_ * scale_, 124 | head_length_ * scale_, head_radius_ * scale_); 125 | } 126 | } 127 | 128 | void MagVisual::setColor(const QColor& color) 129 | { 130 | color_ = color; 131 | if (heading_vector_) 132 | heading_vector_->setColor(color_.redF(), color_.greenF(), 133 | color_.blueF(), alpha_); 134 | } 135 | 136 | void MagVisual::setAlpha(float alpha) 137 | { 138 | alpha_ = alpha; 139 | if (heading_vector_) 140 | heading_vector_->setColor(color_.redF(), color_.greenF(), 141 | color_.blueF(), alpha_); 142 | } 143 | 144 | void MagVisual::setFramePosition(const Ogre::Vector3& position) 145 | { 146 | frame_node_->setPosition(position); 147 | } 148 | 149 | void MagVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 150 | { 151 | frame_node_->setOrientation(orientation); 152 | } 153 | 154 | } // namespace rviz_imu_plugin 155 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_axes_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include 33 | 34 | #include "imu_axes_visual.h" 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | namespace rviz_imu_plugin { 42 | 43 | ImuAxesVisual::ImuAxesVisual(Ogre::SceneManager* scene_manager, 44 | Ogre::SceneNode* parent_node) 45 | : scale_(0.15), quat_valid_(true), orientation_axes_(nullptr) 46 | { 47 | scene_manager_ = scene_manager; 48 | 49 | // Ogre::SceneNode s form a tree, with each node storing the 50 | // transform (position and orientation) of itself relative to its 51 | // parent. Ogre does the math of combining those transforms when it 52 | // is time to render. 53 | // 54 | // Here we create a node to store the pose of the Imu's header frame 55 | // relative to the RViz fixed frame. 56 | frame_node_ = parent_node->createChildSceneNode(); 57 | } 58 | 59 | ImuAxesVisual::~ImuAxesVisual() 60 | { 61 | hide(); 62 | 63 | // Destroy the frame node since we don't need it anymore. 64 | scene_manager_->destroySceneNode(frame_node_); 65 | } 66 | 67 | void ImuAxesVisual::show() 68 | { 69 | if (!orientation_axes_) 70 | { 71 | orientation_axes_ = 72 | new rviz_rendering::Axes(scene_manager_, frame_node_); 73 | orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_)); 74 | orientation_axes_->setOrientation(orientation_); 75 | } 76 | } 77 | 78 | void ImuAxesVisual::hide() 79 | { 80 | if (orientation_axes_) 81 | { 82 | delete orientation_axes_; 83 | orientation_axes_ = nullptr; 84 | } 85 | } 86 | 87 | void ImuAxesVisual::setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg) 88 | { 89 | if (checkQuaternionValidity(msg)) 90 | { 91 | if (!quat_valid_) 92 | { 93 | RVIZ_COMMON_LOG_INFO_STREAM( 94 | "rviz_imu_plugin got valid quaternion, " 95 | "displaying true orientation"); 96 | quat_valid_ = true; 97 | } 98 | orientation_ = Ogre::Quaternion(msg->orientation.w, msg->orientation.x, 99 | msg->orientation.y, msg->orientation.z); 100 | } else 101 | { 102 | if (quat_valid_) 103 | { 104 | RVIZ_COMMON_LOG_WARNING_STREAM( 105 | "rviz_imu_plugin got invalid quaternion (" 106 | << msg->orientation.w << "," << msg->orientation.x << "," 107 | << msg->orientation.y << "," << msg->orientation.z 108 | << "will display neutral orientation instead"); 109 | quat_valid_ = false; 110 | } 111 | // if quaternion is invalid, give a unit quat to Ogre 112 | orientation_ = Ogre::Quaternion(); 113 | } 114 | 115 | if (orientation_axes_) orientation_axes_->setOrientation(orientation_); 116 | } 117 | 118 | void ImuAxesVisual::setScale(float scale) 119 | { 120 | scale_ = scale; 121 | if (orientation_axes_) 122 | orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_)); 123 | } 124 | 125 | void ImuAxesVisual::setFramePosition(const Ogre::Vector3& position) 126 | { 127 | frame_node_->setPosition(position); 128 | } 129 | 130 | void ImuAxesVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 131 | { 132 | frame_node_->setOrientation(orientation); 133 | } 134 | 135 | inline bool ImuAxesVisual::checkQuaternionValidity( 136 | const sensor_msgs::msg::Imu::ConstSharedPtr msg) 137 | { 138 | double x = msg->orientation.x, y = msg->orientation.y, 139 | z = msg->orientation.z, w = msg->orientation.w; 140 | // OGRE can handle unnormalized quaternions, but quat's length extremely 141 | // small; this may indicate that invalid (0, 0, 0, 0) quat is passed, this 142 | // will lead ogre to crash unexpectly 143 | if (std::sqrt(x * x + y * y + z * z + w * w) < 0.0001) 144 | { 145 | return false; 146 | } 147 | return true; 148 | } 149 | 150 | } // namespace rviz_imu_plugin 151 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/mag_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "mag_display.h" 32 | 33 | #include 34 | #include 35 | 36 | namespace rviz_imu_plugin { 37 | 38 | MagDisplay::MagDisplay() 39 | : twod_visual_(true), scene_node_(nullptr), messages_received_(0) 40 | { 41 | createProperties(); 42 | } 43 | 44 | void MagDisplay::onEnable() 45 | { 46 | MessageFilterDisplay::onEnable(); 47 | 48 | mag_visual_->show(); 49 | 50 | scene_node_->setVisible(true); 51 | } 52 | 53 | void MagDisplay::onDisable() 54 | { 55 | MessageFilterDisplay::onDisable(); 56 | 57 | mag_visual_->hide(); 58 | 59 | scene_node_->setVisible(false); 60 | } 61 | 62 | void MagDisplay::onInitialize() 63 | { 64 | MFDClass::onInitialize(); 65 | 66 | // Make an Ogre::SceneNode to contain all our visuals. 67 | scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 68 | 69 | // create magnetic field heading vector visual 70 | mag_visual_ = new MagVisual(context_->getSceneManager(), scene_node_); 71 | 72 | scene_node_->setVisible(isEnabled()); 73 | } 74 | 75 | MagDisplay::~MagDisplay() = default; 76 | 77 | void MagDisplay::reset() 78 | { 79 | MFDClass::reset(); 80 | messages_received_ = 0; 81 | 82 | setStatus(rviz_common::properties::StatusProperty::Warn, "Topic", 83 | "No messages received"); 84 | 85 | mag_visual_->hide(); 86 | } 87 | 88 | void MagDisplay::update(float /* dt */, float /* ros_dt */) 89 | { 90 | updateMag(); 91 | } 92 | 93 | void MagDisplay::updateMag() 94 | { 95 | if (isEnabled()) 96 | mag_visual_->show(); 97 | else 98 | mag_visual_->hide(); 99 | 100 | mag_visual_->setScale(mag_scale_property_->getFloat()); 101 | mag_visual_->setColor(mag_color_property_->getColor()); 102 | mag_visual_->setAlpha(mag_alpha_property_->getFloat()); 103 | mag_visual_->set2d(mag_2d_property_->getBool()); 104 | } 105 | 106 | void MagDisplay::processMessage( 107 | const sensor_msgs::msg::MagneticField::ConstSharedPtr msg) 108 | { 109 | if (!isEnabled()) return; 110 | 111 | ++messages_received_; 112 | 113 | std::stringstream ss; 114 | ss << messages_received_ << " messages received"; 115 | setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", 116 | ss.str().c_str()); 117 | 118 | Ogre::Quaternion orientation; 119 | Ogre::Vector3 position; 120 | if (!context_->getFrameManager()->getTransform( 121 | msg->header.frame_id, msg->header.stamp, position, orientation)) 122 | { 123 | RVIZ_COMMON_LOG_ERROR_STREAM("Error transforming from frame '" 124 | << msg->header.frame_id << "' to frame '" 125 | << fixed_frame_.toStdString() << "'"); 126 | return; 127 | } 128 | 129 | mag_visual_->setMessage(msg); 130 | mag_visual_->setFramePosition(position); 131 | mag_visual_->setFrameOrientation(orientation); 132 | mag_visual_->show(); 133 | } 134 | 135 | void MagDisplay::createProperties() 136 | { 137 | // **** acceleration vector properties 138 | mag_2d_property_ = new rviz_common::properties::BoolProperty( 139 | "2D-visual", twod_visual_, "Use only 2D visualization", this, 140 | SLOT(updateMag())); 141 | 142 | mag_scale_property_ = new rviz_common::properties::FloatProperty( 143 | "Scale", true, "Vector size, in meters", this, SLOT(updateMag())); 144 | mag_color_property_ = new rviz_common::properties::ColorProperty( 145 | "Color", Qt::red, "Color to draw vector.", this, SLOT(updateMag())); 146 | mag_alpha_property_ = new rviz_common::properties::FloatProperty( 147 | "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", this, 148 | SLOT(updateMag())); 149 | } 150 | 151 | } // namespace rviz_imu_plugin 152 | 153 | // Tell pluginlib about this class. It is important to do this in 154 | // global scope, outside our package's namespace. 155 | #include 156 | PLUGINLIB_EXPORT_CLASS(rviz_imu_plugin::MagDisplay, rviz_common::Display) 157 | -------------------------------------------------------------------------------- /imu_filter_madgwick/test/madgwick_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "test_helpers.h" 4 | 5 | #define FILTER_ITERATIONS 10000 6 | 7 | template 8 | void filterStationary(float Ax, float Ay, float Az, float Mx, float My, 9 | float Mz, double& q0, double& q1, double& q2, double& q3) 10 | { 11 | float dt = 0.1; 12 | float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0) 13 | 14 | ImuFilter filter; 15 | filter.setDriftBiasGain(0.0); 16 | filter.setAlgorithmGain(0.1); 17 | 18 | // initialize with some orientation 19 | filter.setOrientation(q0, q1, q2, q3); 20 | filter.setWorldFrame(FRAME); 21 | 22 | for (int i = 0; i < FILTER_ITERATIONS; i++) 23 | { 24 | filter.madgwickAHRSupdate(Gx, Gy, Gz, Ax, Ay, Az, Mx, My, Mz, dt); 25 | } 26 | 27 | filter.getOrientation(q0, q1, q2, q3); 28 | } 29 | 30 | template 31 | void filterStationary(float Ax, float Ay, float Az, double& q0, double& q1, 32 | double& q2, double& q3) 33 | { 34 | float dt = 0.1; 35 | float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0) 36 | 37 | ImuFilter filter; 38 | filter.setDriftBiasGain(0.0); 39 | filter.setAlgorithmGain(0.1); 40 | 41 | // initialize with some orientation 42 | filter.setOrientation(q0, q1, q2, q3); 43 | filter.setWorldFrame(FRAME); 44 | 45 | for (int i = 0; i < FILTER_ITERATIONS; i++) 46 | { 47 | filter.madgwickAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az, dt); 48 | } 49 | 50 | filter.getOrientation(q0, q1, q2, q3); 51 | } 52 | 53 | #define TEST_STATIONARY_ENU(in_am, exp_result) \ 54 | TEST(MadgwickTest, Stationary_ENU_##in_am) \ 55 | { \ 56 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 57 | filterStationary(in_am, q0, q1, q2, q3); \ 58 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 59 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); \ 60 | } \ 61 | TEST(MadgwickTest, Stationary_ENU_NM_##in_am) \ 62 | { \ 63 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 64 | filterStationary(ACCEL_ONLY(in_am), q0, q1, q2, q3); \ 65 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 66 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); \ 67 | } 68 | 69 | #define TEST_STATIONARY_NED(in_am, exp_result) \ 70 | TEST(MadgwickTest, Stationary_NED_##in_am) \ 71 | { \ 72 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 73 | filterStationary(in_am, q0, q1, q2, q3); \ 74 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 75 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); \ 76 | } \ 77 | TEST(MadgwickTest, Stationary_NED_NM_##in_am) \ 78 | { \ 79 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 80 | filterStationary(ACCEL_ONLY(in_am), q0, q1, q2, q3); \ 81 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 82 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); \ 83 | } 84 | 85 | #define TEST_STATIONARY_NWU(in_am, exp_result) \ 86 | TEST(MadgwickTest, Stationary_NWU_##in_am) \ 87 | { \ 88 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 89 | filterStationary(in_am, q0, q1, q2, q3); \ 90 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 91 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); \ 92 | } \ 93 | TEST(MadgwickTest, Stationary_NWU_NM_##in_am) \ 94 | { \ 95 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 96 | filterStationary(ACCEL_ONLY(in_am), q0, q1, q2, q3); \ 97 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 98 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); \ 99 | } 100 | 101 | TEST_STATIONARY_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180) 102 | TEST_STATIONARY_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY) 103 | TEST_STATIONARY_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU) 104 | TEST_STATIONARY_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU) 105 | 106 | TEST_STATIONARY_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY) 107 | TEST_STATIONARY_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120) 108 | TEST_STATIONARY_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90) 109 | TEST_STATIONARY_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU) 110 | TEST_STATIONARY_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU) 111 | 112 | TEST_STATIONARY_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY) 113 | TEST_STATIONARY_NED(AM_NORTH_WEST_UP, QUAT_X_180) 114 | TEST_STATIONARY_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED) 115 | TEST_STATIONARY_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED) 116 | 117 | TEST(MadgwickTest, TestQuatEqNoZ) 118 | { 119 | ASSERT_TRUE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_MZ_90)); 120 | ASSERT_FALSE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_X_180)); 121 | } 122 | 123 | int main(int argc, char** argv) 124 | { 125 | testing::InitGoogleTest(&argc, argv); 126 | return RUN_ALL_TESTS(); 127 | } 128 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_acc_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include 33 | 34 | #include "imu_acc_visual.h" 35 | #include 36 | 37 | namespace rviz_imu_plugin { 38 | 39 | ImuAccVisual::ImuAccVisual(Ogre::SceneManager* scene_manager, 40 | Ogre::SceneNode* parent_node) 41 | : acc_vector_(NULL), 42 | arrow_length_(9.81), 43 | arrow_radius_(0.50), 44 | head_length_(1.00), 45 | head_radius_(1.00), 46 | scale_(0.05), 47 | alpha_(1.0), 48 | color_(1.0, 1.0, 0.0), 49 | derotated_(true) 50 | { 51 | scene_manager_ = scene_manager; 52 | 53 | // Ogre::SceneNode s form a tree, with each node storing the 54 | // transform (position and orientation) of itself relative to its 55 | // parent. Ogre does the math of combining those transforms when it 56 | // is time to render. 57 | // 58 | // Here we create a node to store the pose of the Imu's header frame 59 | // relative to the RViz fixed frame. 60 | frame_node_ = parent_node->createChildSceneNode(); 61 | } 62 | 63 | ImuAccVisual::~ImuAccVisual() 64 | { 65 | hide(); 66 | 67 | // Destroy the frame node since we don't need it anymore. 68 | scene_manager_->destroySceneNode(frame_node_); 69 | } 70 | 71 | void ImuAccVisual::show() 72 | { 73 | if (!acc_vector_) 74 | { 75 | acc_vector_ = new rviz_rendering::Arrow(scene_manager_, frame_node_); 76 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), 77 | alpha_); 78 | acc_vector_->setDirection(direction_); 79 | acc_vector_->set(arrow_length_ * scale_, arrow_radius_ * scale_, 80 | head_length_ * scale_, head_radius_ * scale_); 81 | } 82 | } 83 | 84 | void ImuAccVisual::hide() 85 | { 86 | if (acc_vector_) 87 | { 88 | delete acc_vector_; 89 | acc_vector_ = NULL; 90 | } 91 | } 92 | 93 | void ImuAccVisual::setMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg) 94 | { 95 | direction_ = 96 | Ogre::Vector3(msg->linear_acceleration.x, msg->linear_acceleration.y, 97 | msg->linear_acceleration.z); 98 | 99 | // Rotate the acceleration vector by the IMU orientation. This makes 100 | // sense since the visualization of the IMU is also rotated by the 101 | // orientation. In this way, both appear in the inertial frame. 102 | if (derotated_) 103 | { 104 | Ogre::Quaternion orientation(msg->orientation.w, msg->orientation.x, 105 | msg->orientation.y, msg->orientation.z); 106 | 107 | direction_ = orientation * direction_; 108 | } 109 | 110 | arrow_length_ = 111 | sqrt(msg->linear_acceleration.x * msg->linear_acceleration.x + 112 | msg->linear_acceleration.y * msg->linear_acceleration.y + 113 | msg->linear_acceleration.z * msg->linear_acceleration.z); 114 | 115 | if (acc_vector_) 116 | { 117 | acc_vector_->setDirection(direction_); 118 | acc_vector_->set(arrow_length_ * scale_, arrow_radius_ * scale_, 119 | head_length_ * scale_, head_radius_ * scale_); 120 | } 121 | } 122 | 123 | void ImuAccVisual::setScale(float scale) 124 | { 125 | scale_ = scale; 126 | if (acc_vector_) 127 | { 128 | acc_vector_->setDirection(direction_); 129 | acc_vector_->set(arrow_length_ * scale_, arrow_radius_ * scale_, 130 | head_length_ * scale_, head_radius_ * scale_); 131 | } 132 | } 133 | 134 | void ImuAccVisual::setColor(const QColor& color) 135 | { 136 | color_ = color; 137 | if (acc_vector_) 138 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), 139 | alpha_); 140 | } 141 | 142 | void ImuAccVisual::setAlpha(float alpha) 143 | { 144 | alpha_ = alpha; 145 | if (acc_vector_) 146 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), 147 | alpha_); 148 | } 149 | 150 | void ImuAccVisual::setDerotated(bool derotated) 151 | { 152 | derotated_ = derotated; 153 | if (acc_vector_) 154 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), 155 | alpha_); 156 | } 157 | 158 | void ImuAccVisual::setFramePosition(const Ogre::Vector3& position) 159 | { 160 | frame_node_->setPosition(position); 161 | } 162 | 163 | void ImuAccVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 164 | { 165 | frame_node_->setOrientation(orientation); 166 | } 167 | 168 | } // namespace rviz_imu_plugin 169 | -------------------------------------------------------------------------------- /imu_filter_madgwick/src/stateless_orientation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include "imu_filter_madgwick/stateless_orientation.h" 26 | #include 27 | #include 28 | #include 29 | 30 | template 31 | static inline void crossProduct(T ax, T ay, T az, T bx, T by, T bz, T& rx, 32 | T& ry, T& rz) 33 | { 34 | rx = ay * bz - az * by; 35 | ry = az * bx - ax * bz; 36 | rz = ax * by - ay * bx; 37 | } 38 | 39 | template 40 | static inline T normalizeVector(T& vx, T& vy, T& vz) 41 | { 42 | T norm = sqrt(vx * vx + vy * vy + vz * vz); 43 | T inv = 1.0 / norm; 44 | vx *= inv; 45 | vy *= inv; 46 | vz *= inv; 47 | return norm; 48 | } 49 | 50 | bool StatelessOrientation::computeOrientation( 51 | WorldFrame::WorldFrame frame, geometry_msgs::msg::Vector3 A, 52 | geometry_msgs::msg::Vector3 E, geometry_msgs::msg::Quaternion& orientation) 53 | { 54 | float Hx, Hy, Hz; 55 | float Mx, My, Mz; 56 | float normH; 57 | 58 | // A: pointing up 59 | float Ax = A.x, Ay = A.y, Az = A.z; 60 | 61 | // E: pointing down/north 62 | float Ex = E.x, Ey = E.y, Ez = E.z; 63 | 64 | // H: vector horizontal, pointing east 65 | // H = E x A 66 | crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz); 67 | 68 | // normalize H 69 | normH = normalizeVector(Hx, Hy, Hz); 70 | if (normH < 1E-7) 71 | { 72 | // device is close to free fall (or in space?), or close to 73 | // magnetic north pole. 74 | // mag in T => Threshold 1E-7, typical values are > 1E-5. 75 | return false; 76 | } 77 | 78 | // normalize A 79 | normalizeVector(Ax, Ay, Az); 80 | 81 | // M: vector horizontal, pointing north 82 | // M = A x H 83 | crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz); 84 | 85 | // Create matrix for basis transformation 86 | tf2::Matrix3x3 R; 87 | switch (frame) 88 | { 89 | case WorldFrame::NED: 90 | // vector space world W: 91 | // Basis: bwx (1,0,0) north, bwy (0,1,0) east, bwz (0,0,1) down 92 | // vector space local L: 93 | // Basis: M ,H, -A 94 | // W(1,0,0) => L(M) 95 | // W(0,1,0) => L(H) 96 | // W(0,0,1) => L(-A) 97 | 98 | // R: Transform Matrix local => world equals basis of L, because 99 | // basis of W is I 100 | R[0][0] = Mx; 101 | R[0][1] = Hx; 102 | R[0][2] = -Ax; 103 | R[1][0] = My; 104 | R[1][1] = Hy; 105 | R[1][2] = -Ay; 106 | R[2][0] = Mz; 107 | R[2][1] = Hz; 108 | R[2][2] = -Az; 109 | break; 110 | 111 | case WorldFrame::NWU: 112 | // vector space world W: 113 | // Basis: bwx (1,0,0) north, bwy (0,1,0) west, bwz (0,0,1) up 114 | // vector space local L: 115 | // Basis: M ,H, -A 116 | // W(1,0,0) => L(M) 117 | // W(0,1,0) => L(-H) 118 | // W(0,0,1) => L(A) 119 | 120 | // R: Transform Matrix local => world equals basis of L, because 121 | // basis of W is I 122 | R[0][0] = Mx; 123 | R[0][1] = -Hx; 124 | R[0][2] = Ax; 125 | R[1][0] = My; 126 | R[1][1] = -Hy; 127 | R[1][2] = Ay; 128 | R[2][0] = Mz; 129 | R[2][1] = -Hz; 130 | R[2][2] = Az; 131 | break; 132 | 133 | default: 134 | case WorldFrame::ENU: 135 | // vector space world W: 136 | // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up 137 | // vector space local L: 138 | // Basis: H, M , A 139 | // W(1,0,0) => L(H) 140 | // W(0,1,0) => L(M) 141 | // W(0,0,1) => L(A) 142 | 143 | // R: Transform Matrix local => world equals basis of L, because 144 | // basis of W is I 145 | R[0][0] = Hx; 146 | R[0][1] = Mx; 147 | R[0][2] = Ax; 148 | R[1][0] = Hy; 149 | R[1][1] = My; 150 | R[1][2] = Ay; 151 | R[2][0] = Hz; 152 | R[2][1] = Mz; 153 | R[2][2] = Az; 154 | break; 155 | } 156 | 157 | // Matrix.getRotation assumes vector rotation, but we're using 158 | // coordinate systems. Thus negate rotation angle (inverse). 159 | tf2::Quaternion q; 160 | R.getRotation(q); 161 | tf2::convert(q.inverse(), orientation); 162 | return true; 163 | } 164 | 165 | bool StatelessOrientation::computeOrientation( 166 | WorldFrame::WorldFrame frame, geometry_msgs::msg::Vector3 A, 167 | geometry_msgs::msg::Quaternion& orientation) 168 | { 169 | // This implementation could be optimized regarding speed. 170 | 171 | // magnetic Field E must not be parallel to A, 172 | // choose an arbitrary orthogonal vector 173 | geometry_msgs::msg::Vector3 E; 174 | if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) 175 | { 176 | E.x = A.y; 177 | E.y = A.x; 178 | E.z = 0.0; 179 | } else if (fabs(A.z) > 0.1) 180 | { 181 | E.x = 0.0; 182 | E.y = A.z; 183 | E.z = A.y; 184 | } else 185 | { 186 | // free fall 187 | return false; 188 | } 189 | 190 | return computeOrientation(frame, A, E, orientation); 191 | } 192 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef RVIZ_IMU_PLUGIN_IMU_DISPLAY_H 32 | #define RVIZ_IMU_PLUGIN_IMU_DISPLAY_H 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "imu_axes_visual.h" 49 | #include "imu_orientation_visual.h" 50 | #include "imu_acc_visual.h" 51 | 52 | namespace Ogre { 53 | class SceneNode; 54 | } 55 | 56 | namespace rviz_imu_plugin { 57 | 58 | class ImuDisplay 59 | : public rviz_common::MessageFilterDisplay 60 | { 61 | Q_OBJECT 62 | public: 63 | ImuDisplay(); 64 | ~ImuDisplay() override; 65 | 66 | void onInitialize() override; 67 | void onEnable() override; 68 | void onDisable() override; 69 | 70 | void reset() override; 71 | 72 | void update(float dt, float ros_dt) override; 73 | 74 | private: 75 | void createProperties(); 76 | 77 | const std::string& getTopic() 78 | { 79 | return topic_; 80 | } 81 | 82 | bool getBoxEnabled() 83 | { 84 | return box_enabled_; 85 | } 86 | float getBoxScaleX() 87 | { 88 | return box_visual_->getScaleX(); 89 | } 90 | float getBoxScaleY() 91 | { 92 | return box_visual_->getScaleY(); 93 | } 94 | float getBoxScaleZ() 95 | { 96 | return box_visual_->getScaleZ(); 97 | } 98 | float getBoxAlpha() 99 | { 100 | return box_visual_->getAlpha(); 101 | } 102 | const QColor& getBoxColor() 103 | { 104 | return box_visual_->getColor(); 105 | } 106 | 107 | bool getAxesEnabled() 108 | { 109 | return axes_enabled_; 110 | } 111 | float getAxesScale() 112 | { 113 | return axes_visual_->getScale(); 114 | } 115 | 116 | bool getAccEnabled() 117 | { 118 | return acc_enabled_; 119 | } 120 | float getAccDerotated() 121 | { 122 | return acc_visual_->getDerotated(); 123 | } 124 | float getAccScale() 125 | { 126 | return acc_visual_->getScale(); 127 | } 128 | float getAccAlpha() 129 | { 130 | return acc_visual_->getAlpha(); 131 | } 132 | const QColor& getAccColor() 133 | { 134 | return acc_visual_->getColor(); 135 | } 136 | 137 | protected Q_SLOTS: 138 | 139 | void updateTop(); 140 | void updateBox(); 141 | void updateAxes(); 142 | void updateAcc(); 143 | 144 | private: 145 | // Property objects for user-editable properties. 146 | rviz_common::properties::BoolProperty* fixed_frame_orientation_property_{}; 147 | 148 | rviz_common::properties::Property* box_category_{}; 149 | rviz_common::properties::Property* axes_category_{}; 150 | rviz_common::properties::Property* acc_category_{}; 151 | 152 | // rviz::RosTopicProperty* topic_property_; 153 | rviz_common::properties::BoolProperty* box_enabled_property_{}; 154 | rviz_common::properties::FloatProperty* box_scale_x_property_{}; 155 | rviz_common::properties::FloatProperty* box_scale_y_property_{}; 156 | rviz_common::properties::FloatProperty* box_scale_z_property_{}; 157 | rviz_common::properties::ColorProperty* box_color_property_{}; 158 | rviz_common::properties::FloatProperty* box_alpha_property_{}; 159 | 160 | rviz_common::properties::BoolProperty* axes_enabled_property_{}; 161 | rviz_common::properties::FloatProperty* axes_scale_property_{}; 162 | 163 | rviz_common::properties::BoolProperty* acc_enabled_property_{}; 164 | rviz_common::properties::BoolProperty* acc_derotated_property_{}; 165 | rviz_common::properties::FloatProperty* acc_scale_property_{}; 166 | rviz_common::properties::ColorProperty* acc_color_property_{}; 167 | rviz_common::properties::FloatProperty* acc_alpha_property_{}; 168 | 169 | // Differetn types of visuals 170 | ImuOrientationVisual* box_visual_{}; 171 | ImuAxesVisual* axes_visual_{}; 172 | ImuAccVisual* acc_visual_{}; 173 | 174 | // User-editable property variables. 175 | std::string topic_; 176 | bool fixed_frame_orientation_; 177 | bool box_enabled_; 178 | bool axes_enabled_; 179 | bool acc_enabled_; 180 | 181 | // A node in the Ogre scene tree to be the parent of all our visuals. 182 | Ogre::SceneNode* scene_node_; 183 | 184 | int messages_received_; 185 | 186 | // Function to handle an incoming ROS message. 187 | void processMessage(sensor_msgs::msg::Imu::ConstSharedPtr msg) override; 188 | }; 189 | 190 | } // namespace rviz_imu_plugin 191 | 192 | #endif // RVIZ_IMU_PLUGIN_IMU_DISPLAY_H 193 | -------------------------------------------------------------------------------- /imu_filter_madgwick/test/stateless_orientation_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "test_helpers.h" 3 | 4 | template 5 | bool computeOrientation(float Ax, float Ay, float Az, float Mx, float My, 6 | float Mz, double& q0, double& q1, double& q2, 7 | double& q3) 8 | { 9 | geometry_msgs::msg::Vector3 A, M; 10 | geometry_msgs::msg::Quaternion orientation; 11 | A.x = Ax; 12 | A.y = Ay; 13 | A.z = Az; 14 | M.x = Mx; 15 | M.y = My; 16 | M.z = Mz; 17 | 18 | bool res = 19 | StatelessOrientation::computeOrientation(FRAME, A, M, orientation); 20 | 21 | q0 = orientation.w; 22 | q1 = orientation.x; 23 | q2 = orientation.y; 24 | q3 = orientation.z; 25 | 26 | return res; 27 | } 28 | 29 | template 30 | bool computeOrientation(float Ax, float Ay, float Az, double& q0, double& q1, 31 | double& q2, double& q3) 32 | { 33 | geometry_msgs::msg::Vector3 A; 34 | geometry_msgs::msg::Quaternion orientation; 35 | A.x = Ax; 36 | A.y = Ay; 37 | A.z = Az; 38 | 39 | bool res = StatelessOrientation::computeOrientation(FRAME, A, orientation); 40 | 41 | q0 = orientation.w; 42 | q1 = orientation.x; 43 | q2 = orientation.y; 44 | q3 = orientation.z; 45 | 46 | return res; 47 | } 48 | 49 | #define TEST_STATELESS_ENU(in_am, exp_result) \ 50 | TEST(StatelessOrientationTest, Stationary_ENU_##in_am) \ 51 | { \ 52 | double q0, q1, q2, q3; \ 53 | ASSERT_TRUE( \ 54 | computeOrientation(in_am, q0, q1, q2, q3)); \ 55 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 56 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); \ 57 | } \ 58 | TEST(StatelessOrientationTest, Stationary_ENU_NM_##in_am) \ 59 | { \ 60 | double q0, q1, q2, q3; \ 61 | ASSERT_TRUE(computeOrientation(ACCEL_ONLY(in_am), q0, \ 62 | q1, q2, q3)); \ 63 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 64 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); \ 65 | } 66 | 67 | #define TEST_STATELESS_NED(in_am, exp_result) \ 68 | TEST(StatelessOrientationTest, Stationary_NED_##in_am) \ 69 | { \ 70 | double q0, q1, q2, q3; \ 71 | ASSERT_TRUE( \ 72 | computeOrientation(in_am, q0, q1, q2, q3)); \ 73 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 74 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); \ 75 | } \ 76 | TEST(StatelessOrientationTest, Stationary_NED_NM_##in_am) \ 77 | { \ 78 | double q0, q1, q2, q3; \ 79 | ASSERT_TRUE(computeOrientation(ACCEL_ONLY(in_am), q0, \ 80 | q1, q2, q3)); \ 81 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 82 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); \ 83 | } 84 | 85 | #define TEST_STATELESS_NWU(in_am, exp_result) \ 86 | TEST(StatelessOrientationTest, Stationary_NWU_##in_am) \ 87 | { \ 88 | double q0, q1, q2, q3; \ 89 | ASSERT_TRUE( \ 90 | computeOrientation(in_am, q0, q1, q2, q3)); \ 91 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 92 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); \ 93 | } \ 94 | TEST(StatelessOrientationTest, Stationary_NWU_NM_##in_am) \ 95 | { \ 96 | double q0, q1, q2, q3; \ 97 | ASSERT_TRUE(computeOrientation(ACCEL_ONLY(in_am), q0, \ 98 | q1, q2, q3)); \ 99 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 100 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); \ 101 | } 102 | 103 | TEST_STATELESS_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY) 104 | TEST_STATELESS_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120) 105 | TEST_STATELESS_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90) 106 | TEST_STATELESS_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU) 107 | TEST_STATELESS_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU) 108 | 109 | TEST_STATELESS_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY) 110 | TEST_STATELESS_NED(AM_WEST_NORTH_DOWN, QUAT_MZ_90) 111 | TEST_STATELESS_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED) 112 | TEST_STATELESS_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED) 113 | 114 | TEST_STATELESS_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180) 115 | TEST_STATELESS_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY) 116 | TEST_STATELESS_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU) 117 | TEST_STATELESS_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU) 118 | 119 | TEST(StatelessOrientationTest, Check_NoAccel) 120 | { 121 | double q0, q1, q2, q3; 122 | ASSERT_FALSE(computeOrientation(0.0, 0.0, 0.0, 0.0, 0.0005, 123 | -0.0005, q0, q1, q2, q3)); 124 | } 125 | 126 | TEST(StatelessOrientationTest, Check_NoMag) 127 | { 128 | double q0, q1, q2, q3; 129 | ASSERT_FALSE(computeOrientation(0.0, 0.0, 9.81, 0.0, 0.0, 130 | 0.0, q0, q1, q2, q3)); 131 | } 132 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_orientation_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include 33 | 34 | #include "imu_orientation_visual.h" 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace rviz_imu_plugin { 42 | 43 | ImuOrientationVisual::ImuOrientationVisual(Ogre::SceneManager* scene_manager, 44 | Ogre::SceneNode* parent_node) 45 | : scale_x_(0.07), 46 | scale_y_(0.10), 47 | scale_z_(0.03), 48 | color_(0.5, 0.5, 0.5), 49 | alpha_(1.0), 50 | quat_valid_(true), 51 | orientation_box_(nullptr) 52 | { 53 | scene_manager_ = scene_manager; 54 | 55 | // Ogre::SceneNode s form a tree, with each node storing the 56 | // transform (position and orientation) of itself relative to its 57 | // parent. Ogre does the math of combining those transforms when it 58 | // is time to render. 59 | // 60 | // Here we create a node to store the pose of the Imu's header frame 61 | // relative to the RViz fixed frame. 62 | frame_node_ = parent_node->createChildSceneNode(); 63 | } 64 | 65 | ImuOrientationVisual::~ImuOrientationVisual() 66 | { 67 | hide(); 68 | 69 | // Destroy the frame node since we don't need it anymore. 70 | scene_manager_->destroySceneNode(frame_node_); 71 | } 72 | 73 | void ImuOrientationVisual::show() 74 | { 75 | if (!orientation_box_) 76 | { 77 | orientation_box_ = new rviz_rendering::Shape( 78 | rviz_rendering::Shape::Cube, scene_manager_, frame_node_); 79 | orientation_box_->setColor(color_.redF(), color_.greenF(), 80 | color_.blueF(), alpha_); 81 | orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_)); 82 | orientation_box_->setOrientation(orientation_); 83 | } 84 | } 85 | 86 | void ImuOrientationVisual::hide() 87 | { 88 | if (orientation_box_) 89 | { 90 | delete orientation_box_; 91 | orientation_box_ = nullptr; 92 | } 93 | } 94 | 95 | void ImuOrientationVisual::setMessage( 96 | const sensor_msgs::msg::Imu::ConstSharedPtr msg) 97 | { 98 | if (checkQuaternionValidity(msg)) 99 | { 100 | if (!quat_valid_) 101 | { 102 | RVIZ_COMMON_LOG_INFO_STREAM( 103 | "rviz_imu_plugin got valid quaternion, " 104 | "displaying true orientation"); 105 | quat_valid_ = true; 106 | } 107 | orientation_ = Ogre::Quaternion(msg->orientation.w, msg->orientation.x, 108 | msg->orientation.y, msg->orientation.z); 109 | } else 110 | { 111 | if (quat_valid_) 112 | { 113 | RVIZ_COMMON_LOG_WARNING_STREAM( 114 | "rviz_imu_plugin got invalid quaternion (" 115 | << msg->orientation.w << "," << msg->orientation.x << "," 116 | << msg->orientation.y << "," << msg->orientation.z 117 | << "will display neutral orientation instead"); 118 | quat_valid_ = false; 119 | } 120 | // if quaternion is invalid, give a unit quat to Ogre 121 | orientation_ = Ogre::Quaternion(); 122 | } 123 | 124 | if (orientation_box_) orientation_box_->setOrientation(orientation_); 125 | } 126 | 127 | void ImuOrientationVisual::setScaleX(float x) 128 | { 129 | scale_x_ = x; 130 | if (orientation_box_) 131 | orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_)); 132 | } 133 | 134 | void ImuOrientationVisual::setScaleY(float y) 135 | { 136 | scale_y_ = y; 137 | if (orientation_box_) 138 | orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_)); 139 | } 140 | 141 | void ImuOrientationVisual::setScaleZ(float z) 142 | { 143 | scale_z_ = z; 144 | if (orientation_box_) 145 | orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_)); 146 | } 147 | 148 | void ImuOrientationVisual::setColor(const QColor& color) 149 | { 150 | color_ = color; 151 | if (orientation_box_) 152 | orientation_box_->setColor(color_.redF(), color_.greenF(), 153 | color_.blueF(), alpha_); 154 | } 155 | 156 | void ImuOrientationVisual::setAlpha(float alpha) 157 | { 158 | alpha_ = alpha; 159 | if (orientation_box_) 160 | orientation_box_->setColor(color_.redF(), color_.greenF(), 161 | color_.blueF(), alpha_); 162 | } 163 | 164 | void ImuOrientationVisual::setFramePosition(const Ogre::Vector3& position) 165 | { 166 | frame_node_->setPosition(position); 167 | } 168 | 169 | void ImuOrientationVisual::setFrameOrientation( 170 | const Ogre::Quaternion& orientation) 171 | { 172 | frame_node_->setOrientation(orientation); 173 | } 174 | 175 | inline bool ImuOrientationVisual::checkQuaternionValidity( 176 | const sensor_msgs::msg::Imu::ConstSharedPtr msg) 177 | { 178 | double x = msg->orientation.x, y = msg->orientation.y, 179 | z = msg->orientation.z, w = msg->orientation.w; 180 | // OGRE can handle unnormalized quaternions, but quat's length extremely 181 | // small; this may indicate that invalid (0, 0, 0, 0) quat is passed, this 182 | // will lead ogre to crash unexpectly 183 | if (std::sqrt(x * x + y * y + z * z + w * w) < 0.0001) 184 | { 185 | return false; 186 | } 187 | return true; 188 | } 189 | 190 | } // namespace rviz_imu_plugin 191 | -------------------------------------------------------------------------------- /imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_H 35 | #define IMU_TOOLS_COMPLEMENTARY_FILTER_H 36 | 37 | namespace imu_tools { 38 | 39 | class ComplementaryFilter 40 | { 41 | public: 42 | ComplementaryFilter(); 43 | virtual ~ComplementaryFilter(); 44 | 45 | bool setGainAcc(double gain); 46 | bool setGainMag(double gain); 47 | double getGainAcc() const; 48 | double getGainMag() const; 49 | 50 | bool setBiasAlpha(double bias_alpha); 51 | double getBiasAlpha() const; 52 | 53 | // When the filter is in the steady state, bias estimation will occur (if 54 | // the parameter is enabled). 55 | bool getSteadyState() const; 56 | 57 | void setDoBiasEstimation(bool do_bias_estimation); 58 | bool getDoBiasEstimation() const; 59 | 60 | void setDoAdaptiveGain(bool do_adaptive_gain); 61 | bool getDoAdaptiveGain() const; 62 | 63 | double getAngularVelocityBiasX() const; 64 | double getAngularVelocityBiasY() const; 65 | double getAngularVelocityBiasZ() const; 66 | 67 | // Set the orientation, as a Hamilton Quaternion, of the body frame wrt the 68 | // fixed frame. 69 | void setOrientation(double q0, double q1, double q2, double q3); 70 | 71 | // Get the orientation, as a Hamilton Quaternion, of the body frame wrt the 72 | // fixed frame. 73 | void getOrientation(double& q0, double& q1, double& q2, double& q3) const; 74 | 75 | // Update from accelerometer and gyroscope data. 76 | // [ax, ay, az]: Normalized gravity vector. 77 | // [wx, wy, wz]: Angular veloctiy, in rad / s. 78 | // dt: time delta, in seconds. 79 | void update(double ax, double ay, double az, double wx, double wy, 80 | double wz, double dt); 81 | 82 | // Update from accelerometer, gyroscope, and magnetometer data. 83 | // [ax, ay, az]: Normalized gravity vector. 84 | // [wx, wy, wz]: Angular veloctiy, in rad / s. 85 | // [mx, my, mz]: Magnetic field, units irrelevant. 86 | // dt: time delta, in seconds. 87 | void update(double ax, double ay, double az, double wx, double wy, 88 | double wz, double mx, double my, double mz, double dt); 89 | 90 | private: 91 | static const double kGravity; 92 | static const double gamma_; 93 | // Bias estimation steady state thresholds 94 | static const double kAngularVelocityThreshold; 95 | static const double kAccelerationThreshold; 96 | static const double kDeltaAngularVelocityThreshold; 97 | 98 | // Gain parameter for the complementary filter, belongs in [0, 1]. 99 | double gain_acc_; 100 | double gain_mag_; 101 | 102 | // Bias estimation gain parameter, belongs in [0, 1]. 103 | double bias_alpha_; 104 | 105 | // Parameter whether to do bias estimation or not. 106 | bool do_bias_estimation_; 107 | 108 | // Parameter whether to do adaptive gain or not. 109 | bool do_adaptive_gain_; 110 | 111 | bool initialized_; 112 | bool steady_state_; 113 | 114 | // The orientation as a Hamilton quaternion (q0 is the scalar). Represents 115 | // the orientation of the fixed frame wrt the body frame. 116 | double q0_, q1_, q2_, q3_; 117 | 118 | // Bias in angular velocities; 119 | double wx_prev_, wy_prev_, wz_prev_; 120 | 121 | // Bias in angular velocities; 122 | double wx_bias_, wy_bias_, wz_bias_; 123 | 124 | void updateBiases(double ax, double ay, double az, double wx, double wy, 125 | double wz); 126 | 127 | bool checkState(double ax, double ay, double az, double wx, double wy, 128 | double wz) const; 129 | 130 | void getPrediction(double wx, double wy, double wz, double dt, 131 | double& q0_pred, double& q1_pred, double& q2_pred, 132 | double& q3_pred) const; 133 | 134 | void getMeasurement(double ax, double ay, double az, double& q0_meas, 135 | double& q1_meas, double& q2_meas, double& q3_meas); 136 | 137 | void getMeasurement(double ax, double ay, double az, double mx, double my, 138 | double mz, double& q0_meas, double& q1_meas, 139 | double& q2_meas, double& q3_meas); 140 | 141 | void getAccCorrection(double ax, double ay, double az, double p0, double p1, 142 | double p2, double p3, double& dq0, double& dq1, 143 | double& dq2, double& dq3); 144 | 145 | void getMagCorrection(double mx, double my, double mz, double p0, double p1, 146 | double p2, double p3, double& dq0, double& dq1, 147 | double& dq2, double& dq3); 148 | 149 | double getAdaptiveGain(double alpha, double ax, double ay, double az); 150 | }; 151 | 152 | // Utility math functions: 153 | 154 | void normalizeVector(double& x, double& y, double& z); 155 | 156 | void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3); 157 | 158 | void scaleQuaternion(double gain, double& dq0, double& dq1, double& dq2, 159 | double& dq3); 160 | 161 | void invertQuaternion(double q0, double q1, double q2, double q3, 162 | double& q0_inv, double& q1_inv, double& q2_inv, 163 | double& q3_inv); 164 | 165 | void quaternionMultiplication(double p0, double p1, double p2, double p3, 166 | double q0, double q1, double q2, double q3, 167 | double& r0, double& r1, double& r2, double& r3); 168 | 169 | void rotateVectorByQuaternion(double x, double y, double z, double q0, 170 | double q1, double q2, double q3, double& vx, 171 | double& vy, double& vz); 172 | 173 | } // namespace imu_tools 174 | 175 | #endif // IMU_TOOLS_COMPLEMENTARY_FILTER_H 176 | -------------------------------------------------------------------------------- /rviz_imu_plugin/src/imu_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "imu_display.h" 32 | 33 | #include 34 | #include 35 | namespace rviz_imu_plugin { 36 | 37 | ImuDisplay::ImuDisplay() 38 | : fixed_frame_orientation_(true), 39 | box_enabled_(false), 40 | axes_enabled_(true), 41 | acc_enabled_(false), 42 | scene_node_(nullptr), 43 | messages_received_(0) 44 | { 45 | createProperties(); 46 | } 47 | 48 | void ImuDisplay::onEnable() 49 | { 50 | MessageFilterDisplay::onEnable(); 51 | 52 | if (box_enabled_) 53 | box_visual_->show(); 54 | else 55 | box_visual_->hide(); 56 | 57 | if (axes_enabled_) 58 | axes_visual_->show(); 59 | else 60 | axes_visual_->hide(); 61 | 62 | if (acc_enabled_) 63 | acc_visual_->show(); 64 | else 65 | acc_visual_->hide(); 66 | 67 | scene_node_->setVisible(true); 68 | } 69 | 70 | void ImuDisplay::onDisable() 71 | { 72 | MessageFilterDisplay::onDisable(); 73 | 74 | box_visual_->hide(); 75 | axes_visual_->hide(); 76 | acc_visual_->hide(); 77 | 78 | scene_node_->setVisible(false); 79 | } 80 | 81 | void ImuDisplay::onInitialize() 82 | { 83 | MFDClass::onInitialize(); 84 | 85 | // Make an Ogre::SceneNode to contain all our visuals. 86 | scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 87 | 88 | // create orientation box visual 89 | box_visual_ = 90 | new ImuOrientationVisual(context_->getSceneManager(), scene_node_); 91 | 92 | // create orientation axes visual 93 | axes_visual_ = new ImuAxesVisual(context_->getSceneManager(), scene_node_); 94 | 95 | // create acceleration vector visual 96 | acc_visual_ = new ImuAccVisual(context_->getSceneManager(), scene_node_); 97 | 98 | scene_node_->setVisible(isEnabled()); 99 | } 100 | 101 | ImuDisplay::~ImuDisplay() = default; 102 | 103 | void ImuDisplay::reset() 104 | { 105 | MFDClass::reset(); 106 | messages_received_ = 0; 107 | 108 | setStatus(rviz_common::properties::StatusProperty::Warn, "Topic", 109 | "No messages received"); 110 | 111 | box_visual_->hide(); 112 | axes_visual_->hide(); 113 | acc_visual_->hide(); 114 | } 115 | 116 | void ImuDisplay::update(float /* dt */, float /* ros_dt */) 117 | { 118 | updateTop(); 119 | updateBox(); 120 | updateAxes(); 121 | updateAcc(); 122 | } 123 | 124 | void ImuDisplay::updateTop() 125 | { 126 | fixed_frame_orientation_ = fixed_frame_orientation_property_->getBool(); 127 | } 128 | 129 | void ImuDisplay::updateBox() 130 | { 131 | box_enabled_ = box_enabled_property_->getBool(); 132 | if (isEnabled() && box_enabled_) 133 | box_visual_->show(); 134 | else 135 | box_visual_->hide(); 136 | 137 | box_visual_->setScaleX(box_scale_x_property_->getFloat()); 138 | box_visual_->setScaleY(box_scale_y_property_->getFloat()); 139 | box_visual_->setScaleZ(box_scale_z_property_->getFloat()); 140 | box_visual_->setColor(box_color_property_->getColor()); 141 | box_visual_->setAlpha(box_alpha_property_->getFloat()); 142 | } 143 | 144 | void ImuDisplay::updateAxes() 145 | { 146 | axes_enabled_ = axes_enabled_property_->getBool(); 147 | if (isEnabled() && axes_enabled_) 148 | axes_visual_->show(); 149 | else 150 | axes_visual_->hide(); 151 | 152 | axes_visual_->setScale(axes_scale_property_->getFloat()); 153 | } 154 | 155 | void ImuDisplay::updateAcc() 156 | { 157 | acc_enabled_ = acc_enabled_property_->getBool(); 158 | if (isEnabled() && acc_enabled_) 159 | acc_visual_->show(); 160 | else 161 | acc_visual_->hide(); 162 | 163 | acc_visual_->setScale(acc_scale_property_->getFloat()); 164 | acc_visual_->setColor(acc_color_property_->getColor()); 165 | acc_visual_->setAlpha(acc_alpha_property_->getFloat()); 166 | acc_visual_->setDerotated(acc_derotated_property_->getBool()); 167 | } 168 | 169 | void ImuDisplay::processMessage(const sensor_msgs::msg::Imu::ConstSharedPtr msg) 170 | { 171 | if (!isEnabled()) return; 172 | 173 | ++messages_received_; 174 | 175 | std::stringstream ss; 176 | ss << messages_received_ << " messages received"; 177 | setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", 178 | ss.str().c_str()); 179 | 180 | Ogre::Quaternion orientation; 181 | Ogre::Vector3 position; 182 | if (!context_->getFrameManager()->getTransform( 183 | msg->header.frame_id, msg->header.stamp, position, orientation)) 184 | { 185 | RVIZ_COMMON_LOG_ERROR_STREAM("Error transforming from frame '" 186 | << msg->header.frame_id << "' to frame '" 187 | << fixed_frame_.toStdString() << "'"); 188 | return; 189 | } 190 | 191 | if (fixed_frame_orientation_) 192 | { 193 | /* Display IMU orientation is in the world-fixed frame. */ 194 | Ogre::Vector3 unused; 195 | if (!context_->getFrameManager()->getTransform( 196 | context_->getFrameManager()->getFixedFrame(), msg->header.stamp, 197 | unused, orientation)) 198 | { 199 | RVIZ_COMMON_LOG_ERROR_STREAM("Error getting fixed frame transform"); 200 | return; 201 | } 202 | } 203 | 204 | if (box_enabled_) 205 | { 206 | box_visual_->setMessage(msg); 207 | box_visual_->setFramePosition(position); 208 | box_visual_->setFrameOrientation(orientation); 209 | box_visual_->show(); 210 | } 211 | if (axes_enabled_) 212 | { 213 | axes_visual_->setMessage(msg); 214 | axes_visual_->setFramePosition(position); 215 | axes_visual_->setFrameOrientation(orientation); 216 | axes_visual_->show(); 217 | } 218 | if (acc_enabled_) 219 | { 220 | acc_visual_->setMessage(msg); 221 | acc_visual_->setFramePosition(position); 222 | acc_visual_->setFrameOrientation(orientation); 223 | acc_visual_->show(); 224 | } 225 | } 226 | 227 | void ImuDisplay::createProperties() 228 | { 229 | // **** top level properties 230 | fixed_frame_orientation_property_ = 231 | new rviz_common::properties::BoolProperty( 232 | "fixed_frame_orientation", fixed_frame_orientation_, 233 | "Use world fixed frame for display orientation instead of IMU " 234 | "reference frame", 235 | this, SLOT(updateTop()), this); 236 | 237 | // **** box properties 238 | box_category_ = new rviz_common::properties::Property( 239 | "Box properties", QVariant(), "The list of all the box properties", 240 | this); 241 | box_enabled_property_ = new rviz_common::properties::BoolProperty( 242 | "Enable box", box_enabled_, "Enable the box display", box_category_, 243 | SLOT(updateBox()), this); 244 | box_scale_x_property_ = new rviz_common::properties::FloatProperty( 245 | "x_scale", 1.0, "Box length (x), in meters.", box_category_, 246 | SLOT(updateBox()), this); 247 | box_scale_y_property_ = new rviz_common::properties::FloatProperty( 248 | "y_scale", 1.0, "Box length (y), in meters.", box_category_, 249 | SLOT(updateBox()), this); 250 | box_scale_z_property_ = new rviz_common::properties::FloatProperty( 251 | "z_scale", 1.0, "Box length (z), in meters.", box_category_, 252 | SLOT(updateBox()), this); 253 | box_color_property_ = new rviz_common::properties::ColorProperty( 254 | "Box color", Qt::red, "Color to draw IMU box", box_category_, 255 | SLOT(updateBox()), this); 256 | box_alpha_property_ = new rviz_common::properties::FloatProperty( 257 | "Box alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", 258 | box_category_, SLOT(updateBox()), this); 259 | 260 | // **** axes properties 261 | axes_category_ = new rviz_common::properties::Property( 262 | "Axes properties", QVariant(), "The list of all the axes properties", 263 | this); 264 | axes_enabled_property_ = new rviz_common::properties::BoolProperty( 265 | "Enable axes", axes_enabled_, "Enable the axes display", axes_category_, 266 | SLOT(updateAxes()), this); 267 | axes_scale_property_ = new rviz_common::properties::FloatProperty( 268 | "Axes scale", true, "Axes size, in meters", axes_category_, 269 | SLOT(updateAxes()), this); 270 | 271 | // **** acceleration vector properties 272 | acc_category_ = new rviz_common::properties::Property( 273 | "Acceleration properties", QVariant(), 274 | "The list of all the acceleration properties", this); 275 | acc_enabled_property_ = new rviz_common::properties::BoolProperty( 276 | "Enable acceleration", acc_enabled_, "Enable the acceleration display", 277 | acc_category_, SLOT(updateAcc()), this); 278 | 279 | acc_derotated_property_ = new rviz_common::properties::BoolProperty( 280 | "Derotate acceleration", true, 281 | "If selected, the acceleration is derotated by the IMU orientation. " 282 | "Otherwise, the raw sensor reading is displayed.", 283 | acc_category_, SLOT(updateAcc()), this); 284 | acc_scale_property_ = new rviz_common::properties::FloatProperty( 285 | "Acc. vector scale", true, "Acceleration vector size, in meters", 286 | acc_category_, SLOT(updateAcc()), this); 287 | acc_color_property_ = new rviz_common::properties::ColorProperty( 288 | "Acc. vector color", Qt::red, "Color to draw acceleration vector.", 289 | acc_category_, SLOT(updateAcc()), this); 290 | acc_alpha_property_ = new rviz_common::properties::FloatProperty( 291 | "Acc. vector alpha", 1.0, 292 | "0 is fully transparent, 1.0 is fully opaque.", acc_category_, 293 | SLOT(updateAcc()), this); 294 | } 295 | 296 | } // namespace rviz_imu_plugin 297 | 298 | // Tell pluginlib about this class. It is important to do this in 299 | // global scope, outside our package's namespace. 300 | #include 301 | PLUGINLIB_EXPORT_CLASS(rviz_imu_plugin::ImuDisplay, rviz_common::Display) 302 | -------------------------------------------------------------------------------- /imu_filter_madgwick/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package imu_filter_madgwick 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.1 (2025-05-15) 6 | ------------------ 7 | * Fix deprecated header includes (`#216 `_) 8 | * [kilted] Update deprecated call to ament_target_dependencies (`#215 `_) 9 | * Contributors: David V. Lu!!, Martin Günther 10 | 11 | 2.2.0 (2024-10-01) 12 | ------------------ 13 | 14 | 2.1.4 (2024-04-26) 15 | ------------------ 16 | * Show remapped topic names (`#196 `_) 17 | * Set read-only parameters as read_only (`#185 `_) 18 | * Contributors: Christoph Fröhlich, Tamaki Nishino 19 | 20 | 2.1.3 (2022-12-07) 21 | ------------------ 22 | * Update CMakeLists to use targets 23 | * Remove node\_ prefix. (`#163 `_) 24 | * Contributors: Martin Günther, Max Polzin 25 | 26 | 2.1.2 (2022-07-14) 27 | ------------------ 28 | * Enable on Windows (`#162 `_) 29 | * Contributors: Lou Amadio 30 | 31 | 2.1.1 (2022-05-24) 32 | ------------------ 33 | 34 | 2.1.0 (2022-05-02) 35 | ------------------ 36 | * Switch to non-deprecated hpp header 37 | The .h header became deprecated after galactic. 38 | * Add missing test dependency 39 | * Contributors: Martin Günther 40 | 41 | 2.0.0 (2022-04-12) 42 | ------------------ 43 | * Initial release into ROS2 foxy, galactic and rolling 44 | * Fix gcc warnings + clang-tidy suggestions 45 | * Fix CMakeLists 46 | * Reformat python code using black 47 | * Manually reformat licenses + defines 48 | * Reformat everything using clang-format 49 | * Add license files 50 | The "COPYING" file incorrectly had the text of the LGPL, but the 51 | original Madgwick filter [1], [2] is GPL licensed. The source code 52 | headers correctly have the GPLv3 license text. 53 | [1]: https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 54 | [2]: https://github.com/xioTechnologies/Fusion 55 | * Change to allow the usage of imu_filter_madgwick as a library (`#129 `_) 56 | * imu_filter_madgwick: Install headers 57 | * Remove double configuration steps. (`#122 `_) 58 | Fixes `#118 `_. 59 | * Add declination and yaw offset. (`#121 `_) 60 | Fixes `#120 `_. 61 | * Madgwick for eloquent (`#110 `_) 62 | * Update maintainers in package.xml 63 | * Fix warnings: reordering and unused vars 64 | * Contributors: Martin Günther, boiscljo, tgreier 65 | 66 | 1.2.2 (2020-05-25) 67 | ------------------ 68 | * Drop the signals component of Boost (`#103 `_) 69 | * Add the option to remove the gravity vector (`#101 `_) 70 | * fix install path & boost linkage issues 71 | * Contributors: Alexis Paques, Martin Günther, Mike Purvis, Sean Yen 72 | 73 | 1.2.1 (2019-05-06) 74 | ------------------ 75 | * Skip messages and warn if computeOrientation fails 76 | * Contributors: Martin Günther 77 | 78 | 1.2.0 (2018-05-25) 79 | ------------------ 80 | * Remove outdated Makefile 81 | * Add warning when IMU time stamp is zero 82 | Closes `#82 `_. 83 | * update to use non deprecated pluginlib macro (`#77 `_) 84 | * Contributors: Martin Günther, Mikael Arguedas 85 | 86 | 1.1.5 (2017-05-24) 87 | ------------------ 88 | * Initial release into Lunar 89 | * Remove support for Vector3 mag messages 90 | * Change default world_frame = enu 91 | * Rewrite rosbags: Use MagneticField for magnetometer 92 | * Contributors: Martin Günther 93 | 94 | 1.1.4 (2017-05-22) 95 | ------------------ 96 | * Print warning if waiting for topic 97 | Closes `#61 `_. 98 | * Fix boost::lock_error on shutdown 99 | * Contributors: Martin Günther 100 | 101 | 1.1.3 (2017-03-10) 102 | ------------------ 103 | * Return precisely normalized quaternions 104 | Fixes `#67 `_ : TF_DENORMALIZED_QUATERNION warning added in TF2 0.5.14. 105 | * Tests: Check that output quaternions are normalized 106 | * Fixed lock so it stays in scope until end of method. 107 | * Contributors: Jason Mercer, Martin Guenther, Martin Günther 108 | 109 | 1.1.2 (2016-09-07) 110 | ------------------ 111 | * Add missing dependency on tf2_geometry_msgs 112 | * Contributors: Martin Guenther 113 | 114 | 1.1.1 (2016-09-07) 115 | ------------------ 116 | * Add parameter "world_frame": optionally use ENU or NED instead of NWU 117 | convention (from `#60 `_; 118 | closes `#36 `_) 119 | * Add parameter "stateless" for debugging purposes: don't do any stateful 120 | filtering, but instead publish the orientation directly computed from the 121 | latest accelerometer (+ optionally magnetometer) readings alone 122 | * Replace the (buggy) Euler-angle-based initialization routine 123 | (ImuFilterRos::computeRPY) by a correct transformation 124 | matrix based one (StatelessOrientation::computeOrientation) and make it 125 | available as a library function 126 | * Refactor madgwickAHRSupdate() (pull out some functions, remove micro 127 | optimizations to improve readability) 128 | * Add unit tests 129 | * Contributors: Martin Guenther, Michael Stoll 130 | 131 | 1.1.0 (2016-04-25) 132 | ------------------ 133 | 134 | 1.0.11 (2016-04-22) 135 | ------------------- 136 | * Jade: Change default: use_magnetic_field_msg = true 137 | * Contributors: Martin Guenther 138 | 139 | 1.0.10 (2016-04-22) 140 | ------------------- 141 | 142 | 1.0.9 (2015-10-16) 143 | ------------------ 144 | 145 | 1.0.8 (2015-10-07) 146 | ------------------ 147 | 148 | 1.0.7 (2015-10-07) 149 | ------------------ 150 | 151 | 1.0.6 (2015-10-06) 152 | ------------------ 153 | * Split ImuFilter class into ImuFilter and ImuFilterRos in order to 154 | have a C++ API to the Madgwick algorithm 155 | * Properly install header files. 156 | * Contributors: Martin Günther, Michael Stoll 157 | 158 | 1.0.5 (2015-06-24) 159 | ------------------ 160 | * Add "~use_magnetic_field_msg" param. 161 | This allows the user to subscribe to the /imu/mag topic as a 162 | sensor_msgs/MagneticField rather than a geometry_msgs/Vector3Stamped. 163 | The default for now is false, which preserves the legacy behaviour via a 164 | separate subscriber which converts Vector3Stamped to MagneticField and 165 | republishes. 166 | * Contributors: Mike Purvis, Martin Günther 167 | 168 | 1.0.4 (2015-05-06) 169 | ------------------ 170 | * update dynamic reconfigure param descriptions 171 | * only advertise debug topics if they are used 172 | * allow remapping of the whole imu namespace 173 | with this change, all topics can be remapped at once, like this: 174 | rosrun imu_filter_madgwick imu_filter_node imu:=my_imu 175 | * Contributors: Martin Günther 176 | 177 | 1.0.3 (2015-01-29) 178 | ------------------ 179 | * Add std dev parameter to orientation estimate covariance matrix 180 | * Port imu_filter_madgwick to tf2 181 | * Switch to smart pointer 182 | * Contributors: Paul Bovbel, Martin Günther 183 | 184 | 1.0.2 (2015-01-27) 185 | ------------------ 186 | * fix tf publishing (switch parent + child frames) 187 | The orientation is between a fixed inertial frame (``fixed_frame_``) and 188 | the frame that the IMU is mounted in (``imu_frame_``). Also, 189 | ``imu_msg.header.frame`` should be ``imu_frame_``, but the corresponding TF 190 | goes from ``fixed_frame_`` to ``imu_frame_``. This commit fixes that; for 191 | the ``reverse_tf`` case, it was already correct. 192 | Also see http://answers.ros.org/question/50870/what-frame-is-sensor_msgsimuorientation-relative-to/. 193 | Note that tf publishing should be enabled for debug purposes only, since we can only 194 | provide the orientation, not the translation. 195 | * Add ~reverse_tf parameter for the robots which does not have IMU on root-link 196 | * Log mag bias on startup to assist with debugging. 197 | * add boost depends to CMakeLists 198 | All non-catkin things that we expose in our headers should be added to 199 | the DEPENDS, so that packages which depend on our package will also 200 | automatically link against it. 201 | * Contributors: Martin Günther, Mike Purvis, Ryohei Ueda 202 | 203 | 1.0.1 (2014-12-10) 204 | ------------------ 205 | * add me as maintainer to package.xml 206 | * turn mag_bias into a dynamic reconfigure param 207 | Also rename mag_bias/x --> mag_bias_x etc., since dynamic reconfigure 208 | doesn't allow slashes. 209 | * gain and zeta already set via dynamic_reconfigure 210 | Reading the params explicitly is not necessary. Instead, 211 | dynamic_reconfigure will read them and set them as soon as we call 212 | config_server->setCallback(). 213 | * reconfigure server: use proper namespace 214 | Before, the reconfigure server used the private namespace of the nodelet 215 | *manager* instead of the nodelet, so the params on the parameter server 216 | and the ones from dynamic_reconfigure were out of sync. 217 | * check for NaNs in magnetometer message 218 | Some magnetometer drivers (e.g. phidgets_drivers) output NaNs, which 219 | is a valid way of saying that this measurement is invalid. During 220 | initialization, we simply wait for the first valid message, assuming 221 | there will be one soon. 222 | * magnetometer msg check: isnan() -> !isfinite() 223 | This catches both inf and NaN. Not sure whether sending inf in a Vector3 224 | message is valid (Nan is), but this doesn't hurt and is just good 225 | defensive programming. 226 | * Initialize yaw from calibrated magnetometer data 227 | * Add magnetometer biases (mag_bias/x and mag_bias/y) for hard-iron compensation. 228 | * Initialize yaw orientation from magnetometer reading. 229 | * Add imu/rpy/raw and imu/rpy/filtered as debug topics. imu/rpy/raw can be used for computing magnetometer biases. imu/rpy/filtered topic is for user readability only. 230 | * Contributors: Martin Günther, Shokoofeh Pourmehr 231 | 232 | 1.0.0 (2014-09-03) 233 | ------------------ 234 | * First public release 235 | * Remove setting imu message frame to fixed/odom 236 | * CMakeLists: remove unnecessary link_directories, LIBRARY_OUTPUT_PATH 237 | * add missing build dependency on generated config 238 | This removes a racing condition from the build process. 239 | * install nodelet xml file 240 | Otherwise the nodelet can't be found 241 | * fix implementation of invSqrt() 242 | The old invSqrt() implementation causes the estimate to diverge under 243 | constant input. The problem was the line `long i = (long)&y;`, where 64 244 | bits are read from a 32 bit number. Thanks to @tomas-c for spotting this 245 | and pointing out the solution. 246 | * catkinization of imu_tools metapackage 247 | * fix typo: zeta -> ``zeta_`` 248 | * fix initialization of initial rotation 249 | * gyro drift correction function added in MARG implementation 250 | * set "zeta" as a parameter for dynamic reconfigure in the .cfg file 251 | * add new test bag: phidgets_imu_upside_down 252 | * add parameter publish_tf 253 | When the imu is used together with other packages, such as 254 | robot_pose_ekf, publishing the transform often interferes with those 255 | packages. This parameter allows to disable tf publishing. 256 | * add some sample imu data 257 | * more informative constant_dt message. Reverts to 0.0 on illegal param value 258 | * imu_filter_madgwick manifest now correctly lists the package as GPL license. 259 | * orientation is initialized from acceleration vector on first message received 260 | * added dynamic reconfigure for gain parameter. Added better messages about constant_dt param at startup 261 | * the tf published is now timestamped as the imu msg, and not as now(). Also added constant dt option for the imu+mag callback 262 | * fix the transform publish -- from the fixed frame to the frame of the imu 263 | * add a tf broadcaster with the orientation 264 | * as per PaulKemppi: added option to set constant dt 265 | * walchko: Needed to add namespace: std::isnan() and needed to add rosbuild_link_boost(imu_filter signals) to CMakeLists.txt 266 | * added sebastian's name and link to the manifest 267 | * renamed imu_filter to imu_filter_madgwick 268 | * Contributors: Ivan Dryanovski, Martin Günther, Mike Purvis, Sameer Parekh, TUG-DESTOP, Francisco Vina, Michael Görner, Paul Kemppi, Tomas Cerskus, Kevin Walchko 269 | -------------------------------------------------------------------------------- /imu_complementary_filter/src/complementary_filter_ros.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "imu_complementary_filter/complementary_filter_ros.h" 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace imu_tools { 43 | 44 | ComplementaryFilterROS::ComplementaryFilterROS() 45 | : Node("ComplementaryFilterROS"), 46 | tf_broadcaster_(this), 47 | initialized_filter_(false) 48 | { 49 | RCLCPP_INFO(this->get_logger(), "Starting ComplementaryFilterROS"); 50 | initializeParams(); 51 | 52 | int queue_size = 5; 53 | 54 | // Register publishers: 55 | // TODO: Check why ros::names::resolve is need here 56 | imu_publisher_ = this->create_publisher("imu/data", queue_size); 57 | 58 | if (publish_debug_topics_) 59 | { 60 | rpy_publisher_ = 61 | this->create_publisher( 62 | "imu/rpy/filtered", queue_size); 63 | 64 | if (filter_.getDoBiasEstimation()) 65 | { 66 | state_publisher_ = this->create_publisher( 67 | "imu/steady_state", queue_size); 68 | } 69 | } 70 | 71 | // Register IMU raw data subscriber. 72 | rclcpp::SubscriptionOptions sub_opts; 73 | sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions{{ 74 | rclcpp::QosPolicyKind::Depth, 75 | rclcpp::QosPolicyKind::Durability, 76 | rclcpp::QosPolicyKind::History, 77 | rclcpp::QosPolicyKind::Reliability, 78 | }}; 79 | 80 | imu_subscriber_.reset(new ImuSubscriber(this, "imu/data_raw", 81 | rclcpp::QoS(queue_size), sub_opts)); 82 | 83 | // Register magnetic data subscriber. 84 | if (use_mag_) 85 | { 86 | mag_subscriber_.reset(new MagSubscriber( 87 | this, "imu/mag", rclcpp::QoS(queue_size), sub_opts)); 88 | 89 | sync_.reset(new Synchronizer(SyncPolicy(queue_size), *imu_subscriber_, 90 | *mag_subscriber_)); 91 | sync_->registerCallback(&ComplementaryFilterROS::imuMagCallback, this); 92 | } else 93 | { 94 | imu_subscriber_->registerCallback(&ComplementaryFilterROS::imuCallback, 95 | this); 96 | } 97 | } 98 | 99 | ComplementaryFilterROS::~ComplementaryFilterROS() 100 | { 101 | RCLCPP_INFO(this->get_logger(), "Destroying ComplementaryFilterROS"); 102 | } 103 | 104 | void ComplementaryFilterROS::initializeParams() 105 | { 106 | double gain_acc; 107 | double gain_mag; 108 | bool do_bias_estimation; 109 | double bias_alpha; 110 | bool do_adaptive_gain; 111 | double orientation_stddev; 112 | 113 | // set "Not Dynamically Reconfigurable Parameters" 114 | auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); 115 | descriptor.read_only = true; 116 | 117 | fixed_frame_ = 118 | this->declare_parameter("fixed_frame", "odom", descriptor); 119 | use_mag_ = this->declare_parameter("use_mag", false, descriptor); 120 | publish_tf_ = 121 | this->declare_parameter("publish_tf", false, descriptor); 122 | reverse_tf_ = 123 | this->declare_parameter("reverse_tf", false, descriptor); 124 | constant_dt_ = 125 | this->declare_parameter("constant_dt", 0.0, descriptor); 126 | publish_debug_topics_ = this->declare_parameter( 127 | "publish_debug_topics", false, descriptor); 128 | gain_acc = this->declare_parameter("gain_acc", 0.01, descriptor); 129 | gain_mag = this->declare_parameter("gain_mag", 0.01, descriptor); 130 | do_bias_estimation = 131 | this->declare_parameter("do_bias_estimation", true, descriptor); 132 | bias_alpha = 133 | this->declare_parameter("bias_alpha", 0.01, descriptor); 134 | do_adaptive_gain = 135 | this->declare_parameter("do_adaptive_gain", true, descriptor); 136 | 137 | orientation_stddev = 138 | this->declare_parameter("orientation_stddev", 0.0); 139 | orientation_variance_ = orientation_stddev * orientation_stddev; 140 | 141 | filter_.setDoBiasEstimation(do_bias_estimation); 142 | filter_.setDoAdaptiveGain(do_adaptive_gain); 143 | 144 | if (!filter_.setGainAcc(gain_acc)) 145 | RCLCPP_WARN(this->get_logger(), 146 | "Invalid gain_acc passed to ComplementaryFilter."); 147 | if (use_mag_) 148 | { 149 | if (!filter_.setGainMag(gain_mag)) 150 | RCLCPP_WARN(this->get_logger(), 151 | "Invalid gain_mag passed to ComplementaryFilter."); 152 | } 153 | if (do_bias_estimation) 154 | { 155 | if (!filter_.setBiasAlpha(bias_alpha)) 156 | RCLCPP_WARN(this->get_logger(), 157 | "Invalid bias_alpha passed to ComplementaryFilter."); 158 | } 159 | 160 | // check for illegal constant_dt values 161 | if (constant_dt_ < 0.0) 162 | { 163 | // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt 164 | // otherwise, it will be constant 165 | RCLCPP_WARN( 166 | this->get_logger(), 167 | "constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", 168 | constant_dt_); 169 | constant_dt_ = 0.0; 170 | } 171 | } 172 | 173 | void ComplementaryFilterROS::imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw) 174 | { 175 | const geometry_msgs::msg::Vector3 &a = imu_msg_raw->linear_acceleration; 176 | const geometry_msgs::msg::Vector3 &w = imu_msg_raw->angular_velocity; 177 | const rclcpp::Time &time = imu_msg_raw->header.stamp; 178 | 179 | // Initialize. 180 | if (!initialized_filter_) 181 | { 182 | time_prev_ = time; 183 | initialized_filter_ = true; 184 | return; 185 | } 186 | 187 | // determine dt: either constant, or from IMU timestamp 188 | double dt; 189 | if (constant_dt_ > 0.0) 190 | dt = constant_dt_; 191 | else 192 | dt = (time - time_prev_).nanoseconds() * 1e-9; 193 | 194 | time_prev_ = time; 195 | 196 | // Update the filter. 197 | filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt); 198 | 199 | // Publish state. 200 | publish(imu_msg_raw); 201 | } 202 | 203 | void ComplementaryFilterROS::imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, 204 | MagMsg::ConstSharedPtr mag_msg) 205 | { 206 | const geometry_msgs::msg::Vector3 &a = imu_msg_raw->linear_acceleration; 207 | const geometry_msgs::msg::Vector3 &w = imu_msg_raw->angular_velocity; 208 | const geometry_msgs::msg::Vector3 &m = mag_msg->magnetic_field; 209 | const rclcpp::Time &time = imu_msg_raw->header.stamp; 210 | 211 | // Initialize. 212 | if (!initialized_filter_) 213 | { 214 | time_prev_ = time; 215 | initialized_filter_ = true; 216 | return; 217 | } 218 | 219 | // Calculate dt. 220 | double dt = (time - time_prev_).nanoseconds() * 1e-9; 221 | time_prev_ = time; 222 | // ros::Time t_in, t_out; 223 | // t_in = ros::Time::now(); 224 | // Update the filter. 225 | if (std::isnan(m.x) || std::isnan(m.y) || std::isnan(m.z)) 226 | filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt); 227 | else 228 | filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, m.x, m.y, m.z, dt); 229 | 230 | // t_out = ros::Time::now(); 231 | // float dt_tot = (t_out - t_in).toSec() * 1000.0; // In msec. 232 | // printf("%.6f\n", dt_tot); 233 | // Publish state. 234 | publish(imu_msg_raw); 235 | } 236 | 237 | tf2::Quaternion ComplementaryFilterROS::hamiltonToTFQuaternion(double q0, 238 | double q1, 239 | double q2, 240 | double q3) const 241 | { 242 | // ROS uses the Hamilton quaternion convention (q0 is the scalar). However, 243 | // the ROS quaternion is in the form [x, y, z, w], with w as the scalar. 244 | return tf2::Quaternion(q1, q2, q3, q0); 245 | } 246 | 247 | void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) 248 | { 249 | // Get the orientation: 250 | double q0, q1, q2, q3; 251 | filter_.getOrientation(q0, q1, q2, q3); 252 | tf2::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3); 253 | 254 | // Create and publish fitlered IMU message. 255 | ImuMsg::SharedPtr imu_msg = std::make_shared(*imu_msg_raw); 256 | imu_msg->orientation.x = q1; 257 | imu_msg->orientation.y = q2; 258 | imu_msg->orientation.z = q3; 259 | imu_msg->orientation.w = q0; 260 | 261 | imu_msg->orientation_covariance[0] = orientation_variance_; 262 | imu_msg->orientation_covariance[1] = 0.0; 263 | imu_msg->orientation_covariance[2] = 0.0; 264 | imu_msg->orientation_covariance[3] = 0.0; 265 | imu_msg->orientation_covariance[4] = orientation_variance_; 266 | imu_msg->orientation_covariance[5] = 0.0; 267 | imu_msg->orientation_covariance[6] = 0.0; 268 | imu_msg->orientation_covariance[7] = 0.0; 269 | imu_msg->orientation_covariance[8] = orientation_variance_; 270 | 271 | // Account for biases. 272 | if (filter_.getDoBiasEstimation()) 273 | { 274 | imu_msg->angular_velocity.x -= filter_.getAngularVelocityBiasX(); 275 | imu_msg->angular_velocity.y -= filter_.getAngularVelocityBiasY(); 276 | imu_msg->angular_velocity.z -= filter_.getAngularVelocityBiasZ(); 277 | } 278 | 279 | imu_publisher_->publish(*imu_msg); 280 | 281 | if (publish_debug_topics_) 282 | { 283 | // Create and publish roll, pitch, yaw angles 284 | geometry_msgs::msg::Vector3Stamped rpy; 285 | rpy.header = imu_msg_raw->header; 286 | 287 | tf2::Matrix3x3 M; 288 | M.setRotation(q); 289 | M.getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z); 290 | rpy_publisher_->publish(rpy); 291 | 292 | // Publish whether we are in the steady state, when doing bias 293 | // estimation 294 | if (filter_.getDoBiasEstimation()) 295 | { 296 | std_msgs::msg::Bool state_msg; 297 | state_msg.data = filter_.getSteadyState(); 298 | state_publisher_->publish(state_msg); 299 | } 300 | } 301 | 302 | if (publish_tf_) 303 | { 304 | // Create and publish the ROS tf. 305 | geometry_msgs::msg::TransformStamped transform; 306 | transform.header.stamp = imu_msg_raw->header.stamp; 307 | transform.transform.rotation.x = q1; 308 | transform.transform.rotation.y = q2; 309 | transform.transform.rotation.z = q3; 310 | transform.transform.rotation.w = q0; 311 | 312 | if (reverse_tf_) 313 | { 314 | transform.header.frame_id = imu_msg_raw->header.frame_id; 315 | transform.child_frame_id = fixed_frame_; 316 | tf_broadcaster_.sendTransform(transform); 317 | } else 318 | { 319 | transform.child_frame_id = imu_msg_raw->header.frame_id; 320 | transform.header.frame_id = fixed_frame_; 321 | tf_broadcaster_.sendTransform(transform); 322 | } 323 | } 324 | } 325 | 326 | } // namespace imu_tools 327 | -------------------------------------------------------------------------------- /imu_filter_madgwick/src/imu_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include 26 | #include "imu_filter_madgwick/imu_filter.h" 27 | 28 | // Fast inverse square-root 29 | // See: 30 | // http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root 31 | static float invSqrt(float x) 32 | { 33 | float xhalf = 0.5f * x; 34 | union { 35 | float x; 36 | int i; 37 | } u; 38 | u.x = x; 39 | u.i = 0x5f3759df - (u.i >> 1); 40 | /* The next line can be repeated any number of times to increase accuracy */ 41 | u.x = u.x * (1.5f - xhalf * u.x * u.x); 42 | return u.x; 43 | } 44 | 45 | template 46 | static inline void normalizeVector(T& vx, T& vy, T& vz) 47 | { 48 | T recipNorm = invSqrt(vx * vx + vy * vy + vz * vz); 49 | vx *= recipNorm; 50 | vy *= recipNorm; 51 | vz *= recipNorm; 52 | } 53 | 54 | template 55 | static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3) 56 | { 57 | T recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 58 | q0 *= recipNorm; 59 | q1 *= recipNorm; 60 | q2 *= recipNorm; 61 | q3 *= recipNorm; 62 | } 63 | 64 | static inline void rotateAndScaleVector(float q0, float q1, float q2, float q3, 65 | float _2dx, float _2dy, float _2dz, 66 | float& rx, float& ry, float& rz) 67 | { 68 | // result is half as long as input 69 | rx = _2dx * (0.5f - q2 * q2 - q3 * q3) + _2dy * (q0 * q3 + q1 * q2) + 70 | _2dz * (q1 * q3 - q0 * q2); 71 | ry = _2dx * (q1 * q2 - q0 * q3) + _2dy * (0.5f - q1 * q1 - q3 * q3) + 72 | _2dz * (q0 * q1 + q2 * q3); 73 | rz = _2dx * (q0 * q2 + q1 * q3) + _2dy * (q2 * q3 - q0 * q1) + 74 | _2dz * (0.5f - q1 * q1 - q2 * q2); 75 | } 76 | 77 | static inline void compensateGyroDrift(float q0, float q1, float q2, float q3, 78 | float s0, float s1, float s2, float s3, 79 | float dt, float zeta, float& w_bx, 80 | float& w_by, float& w_bz, float& gx, 81 | float& gy, float& gz) 82 | { 83 | // w_err = 2 q x s 84 | float w_err_x = 85 | 2.0f * q0 * s1 - 2.0f * q1 * s0 - 2.0f * q2 * s3 + 2.0f * q3 * s2; 86 | float w_err_y = 87 | 2.0f * q0 * s2 + 2.0f * q1 * s3 - 2.0f * q2 * s0 - 2.0f * q3 * s1; 88 | float w_err_z = 89 | 2.0f * q0 * s3 - 2.0f * q1 * s2 + 2.0f * q2 * s1 - 2.0f * q3 * s0; 90 | 91 | w_bx += w_err_x * dt * zeta; 92 | w_by += w_err_y * dt * zeta; 93 | w_bz += w_err_z * dt * zeta; 94 | 95 | gx -= w_bx; 96 | gy -= w_by; 97 | gz -= w_bz; 98 | } 99 | 100 | static inline void orientationChangeFromGyro(float q0, float q1, float q2, 101 | float q3, float gx, float gy, 102 | float gz, float& qDot1, 103 | float& qDot2, float& qDot3, 104 | float& qDot4) 105 | { 106 | // Rate of change of quaternion from gyroscope 107 | // See EQ 12 108 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 109 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 110 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 111 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 112 | } 113 | 114 | static inline void addGradientDescentStep(float q0, float q1, float q2, 115 | float q3, float _2dx, float _2dy, 116 | float _2dz, float mx, float my, 117 | float mz, float& s0, float& s1, 118 | float& s2, float& s3) 119 | { 120 | float f0, f1, f2; 121 | 122 | // Gradient decent algorithm corrective step 123 | // EQ 15, 21 124 | rotateAndScaleVector(q0, q1, q2, q3, _2dx, _2dy, _2dz, f0, f1, f2); 125 | 126 | f0 -= mx; 127 | f1 -= my; 128 | f2 -= mz; 129 | 130 | // EQ 22, 34 131 | // Jt * f 132 | s0 += (_2dy * q3 - _2dz * q2) * f0 + (-_2dx * q3 + _2dz * q1) * f1 + 133 | (_2dx * q2 - _2dy * q1) * f2; 134 | s1 += (_2dy * q2 + _2dz * q3) * f0 + 135 | (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1 + 136 | (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2; 137 | s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0 + 138 | (_2dx * q1 + _2dz * q3) * f1 + 139 | (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2; 140 | s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0 + 141 | (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1 + 142 | (_2dx * q1 + _2dy * q2) * f2; 143 | } 144 | 145 | static inline void compensateMagneticDistortion(float q0, float q1, float q2, 146 | float q3, float mx, float my, 147 | float mz, float& _2bxy, 148 | float& _2bz) 149 | { 150 | float hx, hy, hz; 151 | // Reference direction of Earth's magnetic field (See EQ 46) 152 | rotateAndScaleVector(q0, -q1, -q2, -q3, mx, my, mz, hx, hy, hz); 153 | 154 | _2bxy = 4.0f * std::sqrt(hx * hx + hy * hy); 155 | _2bz = 4.0f * hz; 156 | } 157 | 158 | ImuFilter::ImuFilter() 159 | : gain_(0.0), 160 | zeta_(0.0), 161 | world_frame_(WorldFrame::ENU), 162 | q0(1.0), 163 | q1(0.0), 164 | q2(0.0), 165 | q3(0.0), 166 | w_bx_(0.0), 167 | w_by_(0.0), 168 | w_bz_(0.0) 169 | { 170 | } 171 | 172 | ImuFilter::~ImuFilter() = default; 173 | 174 | void ImuFilter::madgwickAHRSupdate(float gx, float gy, float gz, float ax, 175 | float ay, float az, float mx, float my, 176 | float mz, float dt) 177 | { 178 | float s0, s1, s2, s3; 179 | float qDot1, qDot2, qDot3, qDot4; 180 | float _2bz, _2bxy; 181 | 182 | // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in 183 | // magnetometer normalisation) 184 | if (!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz)) 185 | { 186 | madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt); 187 | return; 188 | } 189 | 190 | // Compute feedback only if accelerometer measurement valid (avoids NaN in 191 | // accelerometer normalisation) 192 | if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) 193 | { 194 | // Normalise accelerometer measurement 195 | normalizeVector(ax, ay, az); 196 | 197 | // Normalise magnetometer measurement 198 | normalizeVector(mx, my, mz); 199 | 200 | // Compensate for magnetic distortion 201 | compensateMagneticDistortion(q0, q1, q2, q3, mx, my, mz, _2bxy, _2bz); 202 | 203 | // Gradient decent algorithm corrective step 204 | s0 = 0.0; 205 | s1 = 0.0; 206 | s2 = 0.0; 207 | s3 = 0.0; 208 | switch (world_frame_) 209 | { 210 | case WorldFrame::NED: 211 | // Gravity: [0, 0, -1] 212 | addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, 213 | az, s0, s1, s2, s3); 214 | 215 | // Earth magnetic field: = [bxy, 0, bz] 216 | addGradientDescentStep(q0, q1, q2, q3, _2bxy, 0.0, _2bz, mx, my, 217 | mz, s0, s1, s2, s3); 218 | break; 219 | case WorldFrame::NWU: 220 | // Gravity: [0, 0, 1] 221 | addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, 222 | az, s0, s1, s2, s3); 223 | 224 | // Earth magnetic field: = [bxy, 0, bz] 225 | addGradientDescentStep(q0, q1, q2, q3, _2bxy, 0.0, _2bz, mx, my, 226 | mz, s0, s1, s2, s3); 227 | break; 228 | default: 229 | case WorldFrame::ENU: 230 | // Gravity: [0, 0, 1] 231 | addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, 232 | az, s0, s1, s2, s3); 233 | 234 | // Earth magnetic field: = [0, bxy, bz] 235 | addGradientDescentStep(q0, q1, q2, q3, 0.0, _2bxy, _2bz, mx, my, 236 | mz, s0, s1, s2, s3); 237 | break; 238 | } 239 | normalizeQuaternion(s0, s1, s2, s3); 240 | 241 | // compute gyro drift bias 242 | compensateGyroDrift(q0, q1, q2, q3, s0, s1, s2, s3, dt, zeta_, w_bx_, 243 | w_by_, w_bz_, gx, gy, gz); 244 | 245 | orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, 246 | qDot3, qDot4); 247 | 248 | // Apply feedback step 249 | qDot1 -= gain_ * s0; 250 | qDot2 -= gain_ * s1; 251 | qDot3 -= gain_ * s2; 252 | qDot4 -= gain_ * s3; 253 | } else 254 | { 255 | orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, 256 | qDot3, qDot4); 257 | } 258 | 259 | // Integrate rate of change of quaternion to yield quaternion 260 | q0 += qDot1 * dt; 261 | q1 += qDot2 * dt; 262 | q2 += qDot3 * dt; 263 | q3 += qDot4 * dt; 264 | 265 | // Normalise quaternion 266 | normalizeQuaternion(q0, q1, q2, q3); 267 | } 268 | 269 | void ImuFilter::madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, 270 | float ay, float az, float dt) 271 | { 272 | float s0, s1, s2, s3; 273 | float qDot1, qDot2, qDot3, qDot4; 274 | 275 | // Rate of change of quaternion from gyroscope 276 | orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, 277 | qDot4); 278 | 279 | // Compute feedback only if accelerometer measurement valid (avoids NaN in 280 | // accelerometer normalisation) 281 | if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) 282 | { 283 | // Normalise accelerometer measurement 284 | normalizeVector(ax, ay, az); 285 | 286 | // Gradient decent algorithm corrective step 287 | s0 = 0.0; 288 | s1 = 0.0; 289 | s2 = 0.0; 290 | s3 = 0.0; 291 | switch (world_frame_) 292 | { 293 | case WorldFrame::NED: 294 | // Gravity: [0, 0, -1] 295 | addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, 296 | az, s0, s1, s2, s3); 297 | break; 298 | case WorldFrame::NWU: 299 | case WorldFrame::ENU: 300 | default: 301 | // Gravity: [0, 0, 1] 302 | addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, 303 | az, s0, s1, s2, s3); 304 | break; 305 | } 306 | 307 | normalizeQuaternion(s0, s1, s2, s3); 308 | 309 | // Apply feedback step 310 | qDot1 -= gain_ * s0; 311 | qDot2 -= gain_ * s1; 312 | qDot3 -= gain_ * s2; 313 | qDot4 -= gain_ * s3; 314 | } 315 | 316 | // Integrate rate of change of quaternion to yield quaternion 317 | q0 += qDot1 * dt; 318 | q1 += qDot2 * dt; 319 | q2 += qDot3 * dt; 320 | q3 += qDot4 * dt; 321 | 322 | // Normalise quaternion 323 | normalizeQuaternion(q0, q1, q2, q3); 324 | } 325 | 326 | void ImuFilter::getGravity(float& rx, float& ry, float& rz, float gravity) 327 | { 328 | // Estimate gravity vector from current orientation 329 | switch (world_frame_) 330 | { 331 | case WorldFrame::NED: 332 | // Gravity: [0, 0, -1] 333 | rotateAndScaleVector(q0, q1, q2, q3, 0.0, 0.0, -2.0 * gravity, rx, 334 | ry, rz); 335 | break; 336 | case WorldFrame::NWU: 337 | case WorldFrame::ENU: 338 | default: 339 | // Gravity: [0, 0, 1] 340 | rotateAndScaleVector(q0, q1, q2, q3, 0.0, 0.0, 2.0 * gravity, rx, 341 | ry, rz); 342 | break; 343 | } 344 | } 345 | --------------------------------------------------------------------------------