提交初始版本代码

.gitignore

@@ -0,0 +1,60 @@
+# ---> Python
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+

iic_6dof_imu/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(iic_6dof_imu)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  sensor_msgs
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetYawData.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  CATKIN_DEPENDS 
+  rospy 
+  sensor_msgs
+  std_msgs
+  message_runtime
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/iic_mode_imu.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/iic_mode_imu_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_iic_mode_imu.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

iic_6dof_imu/cfg/param.yaml

@@ -0,0 +1,22 @@
+###################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:使用IIC总线来读取IMU模块的数据,并
+#   通过话题将imu数据发送出来.这里是可以配置的
+#   一些参数.各参数意义如下:
+#   pub_data_topic:发布imu数据的话题名.
+#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
+#   pitch_topic:发布俯仰角的话题名,单位弧度.
+#   roll_topic:发布横滚角的话题名,单位弧度.
+#   link_name:imu模块的urdf中link名字.
+#   pub_hz:上述各话题发布的频率,默认100hz.
+# Author: corvin
+# History:
+#   20211122:init this file.
+###################################################
+topic_pub_hz: 100
+pub_data_topic: imu_data
+yaw_topic: yaw_data
+pitch_topic: pitch_data
+roll_topic: roll_data
+link_name: base_imu_link
+get_yaw_service: /iic_imu_service/getYawData_service

iic_6dof_imu/launch/imu_data_pub.launch

@@ -0,0 +1,15 @@
+<!---
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  Description:该launch启动文件是为了使其进入正常工作状态.启动后就会在
+  /imu_data话题上发布imu传感器信息.需要该信息的节点,只需订阅该话题即可.
+  Author: corvin
+  History:
+    20211122:Initial this launch file.
+-->
+<launch>
+  <arg name="iic_imu_cfg_file" default="$(find iic_6dof_imu)/cfg/param.yaml"/>
+  <node pkg="iic_6dof_imu" type="iic_6dof_imu_node.py" name="iic_6dof_imu_node" output="screen">
+      <rosparam file="$(arg iic_imu_cfg)" command="load"/>
+  </node>
+</launch>
+

iic_6dof_imu/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>iic_6dof_imu</name>
+  <version>0.0.0</version>
+  <description>The iic_6dof_imu package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/iic_6dof_imu</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

iic_6dof_imu/src/iic_6dof_imu_data.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# Author: corvin
+# Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
+# History:
+#    20211122: Initial this file.
+import smbus
+import rospy
+import numpy as np
+
+class MyIMU(object):
+    def __init__(self, addr):
+        self.addr = addr
+        self.i2c = smbus.SMBus(1)
+
+    def get_YPRAG(self):
+        try:
+            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
+            axyz_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
+            gxyz_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
+        except IOError:
+            rospy.logerr("Read IMU RPYAG date error !")
+        else:
+            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])/32768.0*180.0)
+            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
+            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)
+
+            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
+            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
+            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)
+
+            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
+            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
+            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)
+
+    def get_quatern(self):
+        try:
+            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
+        except IOError:
+            rospy.logerr("Read IMU quaternion date error !")
+        else:
+            self.raw_q0 = float((np.short(np.short(quat_data[1]<<8)|quat_data[0]))/32768.0)
+            self.raw_q1 = float((np.short(np.short(quat_data[3]<<8)|quat_data[2]))/32768.0)
+            self.raw_q2 = float((np.short(np.short(quat_data[5]<<8)|quat_data[4]))/32768.0)
+            self.raw_q3 = float((np.short(np.short(quat_data[7]<<8)|quat_data[6]))/32768.0)

iic_6dof_imu/src/iic_6dof_imu_node.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数，
+#    然后组装成ROS中的IMU消息格式，发布到/imu_data话题中，这样有需要的
+#    节点可以直接订阅该话题即可获取到imu当前的数据.
+# Author: corvin
+# History:
+#    20211122:Initial this file.
+import rospy
+import math
+
+from std_msgs.msg import Float32
+from sensor_msgs.msg import Imu
+from iic_6dof_imu_data import MyIMU
+from iic_6dof_imu.srv import GetYawData,GetYawDataResponse
+
+degrees2rad = math.pi/180.0
+yaw = 0.0
+
+# ros server return Yaw data
+def return_yaw_data(req):
+    return GetYawDataResponse(yaw)
+
+rospy.init_node("iic_6dof_imu_node")
+
+# Get DIY config param
+topic_pub_hz    = rospy.get_param('~topic_pub_hz', '100')
+data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
+link_name       = rospy.get_param('~link_name', 'base_imu_link')
+yaw_topic_name  = rospy.get_param('~yaw_topic', 'yaw_topic')
+pitch_topic_name= rospy.get_param('~pitch_topic', 'pitch_topic')
+roll_topic_name = rospy.get_param('~roll_topic', 'roll_topic')
+get_yaw_srv     = rospy.get_param('~get_yaw_service', '/iic_imu_service/getYawData_service')
+
+#创建各话题发布者和自定义服务
+data_pub  = rospy.Publisher(data_topic_name,  Imu,     queue_size=1)
+yaw_pub   = rospy.Publisher(yaw_topic_name,   Float32, queue_size=1)
+pitch_pub = rospy.Publisher(pitch_topic_name, Float32, queue_size=1)
+roll_pub  = rospy.Publisher(roll_topic_name,  Float32, queue_size=1)
+yaw_srv   = rospy.Service(get_yaw_srv, GetYawData, return_yaw_data)
+
+#创建各话题发布的消息
+imuMsg   = Imu()
+yawMsg   = Float32()
+pitchMsg = Float32()
+rollMsg  = Float32()
+
+# Orientation covariance estimation
+imuMsg.orientation_covariance = [
+0.0025 , 0 , 0,
+0, 0.0025, 0,
+0, 0, 0.0025
+]
+
+# Angular velocity covariance estimation
+imuMsg.angular_velocity_covariance = [
+0.02, 0 , 0,
+0 , 0.02, 0,
+0 , 0 , 0.02
+]
+
+# linear acceleration covariance estimation
+imuMsg.linear_acceleration_covariance = [
+0.04 , 0 , 0,
+0 , 0.04, 0,
+0 , 0 , 0.04
+]
+
+seq = 0
+accel_factor = 9.806  #sensor accel g convert to m/s^2.
+
+myIMU = MyIMU(0x50)
+rate = rospy.Rate(topic_pub_hz)
+
+rospy.loginfo("Now 6DOF IMU Module is working ...")
+while not rospy.is_shutdown():
+    myIMU.get_YPRAG()
+
+    #rospy.loginfo("yaw:%f pitch:%f roll:%f", myIMU.raw_yaw,
+    #              myIMU.raw_pitch, myIMU.raw_roll)
+    yaw_deg = float(myIMU.raw_yaw)
+    if yaw_deg >= 180.0:
+        yaw_deg -= 360.0
+
+    #rospy.loginfo("yaw_deg: %f", yaw_deg)
+    yaw   = yaw_deg*degrees2rad
+    pitch = float(myIMU.raw_pitch)*degrees2rad
+    roll  = float(myIMU.raw_roll)*degrees2rad
+
+    #填充话题消息,然后通过话题发布
+    yawMsg.data   = yaw
+    pitchMsg.data = pitch
+    rollMsg.data  = roll
+    yaw_pub.publish(yawMsg)
+    pitch_pub.publish(pitchMsg)
+    roll_pub.publish(rollMsg)
+
+    # Publish imu message
+    #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
+    #              myIMU.raw_ay, myIMU.raw_az)
+    imuMsg.linear_acceleration.x = -float(myIMU.raw_ax)*accel_factor
+    imuMsg.linear_acceleration.y = -float(myIMU.raw_ay)*accel_factor
+    imuMsg.linear_acceleration.z = -float(myIMU.raw_az)*accel_factor
+
+    imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
+    imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
+    imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
+
+    # From IMU module get quatern param
+    myIMU.get_quatern()
+    imuMsg.orientation.x = myIMU.raw_q1
+    imuMsg.orientation.y = myIMU.raw_q2
+    imuMsg.orientation.z = myIMU.raw_q3
+    imuMsg.orientation.w = myIMU.raw_q0
+
+    imuMsg.header.stamp    = rospy.Time.now()
+    imuMsg.header.frame_id = link_name
+    imuMsg.header.seq = seq
+    seq = seq + 1
+    data_pub.publish(imuMsg)
+
+    rate.sleep()

iic_6dof_imu/srv/GetYawData.srv

@@ -0,0 +1,2 @@
+---
+float32 yaw

imu_tools/.gitignore

@@ -0,0 +1,15 @@
+.cproject
+.project
+.pydevproject
+cmake_install.cmake
+bin/
+build/
+lib/
+*.cfgc
+imu_filter_madgwick/cfg/cpp/
+imu_filter_madgwick/docs/ImuFilterMadgwickConfig-usage.dox
+imu_filter_madgwick/docs/ImuFilterMadgwickConfig.dox
+imu_filter_madgwick/docs/ImuFilterMadgwickConfig.wikidoc
+imu_filter_madgwick/src/imu_filter_madgwick/__init__.py
+imu_filter_madgwick/src/imu_filter_madgwick/cfg/
+*~

imu_tools/.travis.yml

@@ -0,0 +1,15 @@
+sudo: required
+
+services:
+  - docker
+
+env:
+  matrix:
+    - CI_ROS_DISTRO="melodic"
+    - CI_ROS_DISTRO="noetic"
+
+install:
+  - docker build -t imu_tools_$CI_ROS_DISTRO -f Dockerfile-$CI_ROS_DISTRO .
+
+script:
+  - docker run imu_tools_$CI_ROS_DISTRO /bin/bash -c "source devel/setup.bash && catkin run_tests && catkin_test_results"

imu_tools/Dockerfile-melodic

@@ -0,0 +1,18 @@
+FROM ros:melodic-ros-core
+
+RUN apt-get update \
+    && apt-get install -y build-essential clang-format python-catkin-tools python-rosdep \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+# Create ROS workspace
+COPY . /ws/src/imu_tools
+WORKDIR /ws
+
+# Use rosdep to install all dependencies (including ROS itself)
+RUN rosdep init && rosdep update && rosdep install --from-paths src -i -y --rosdistro melodic
+
+RUN /bin/bash -c "source /opt/ros/melodic/setup.bash && \
+    catkin init && \
+    catkin config --install -j 1 -p 1 && \
+    catkin build --limit-status-rate 0.1 --no-notify && \
+    catkin build --limit-status-rate 0.1 --no-notify --make-args tests"

imu_tools/Dockerfile-noetic

@@ -0,0 +1,31 @@
+FROM ros:noetic-ros-core
+
+RUN apt-get update \
+    && apt-get install -y build-essential file clang-format python3-rosdep \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+# workaround for https://github.com/catkin/catkin_tools/issues/594:
+# apt-get install python3-catkin-tools doesn't work because python3-trollius doesn't exist on Ubuntu Focal
+
+ENV PATH="/root/.local/bin:${PATH}"
+
+RUN apt-get update \
+    && apt-get install -y git python3-pip \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+RUN pip3 install --user git+https://github.com/catkin/catkin_tools.git
+
+# end workaround
+
+# Create ROS workspace
+COPY . /ws/src/imu_tools
+WORKDIR /ws
+
+# Use rosdep to install all dependencies (including ROS itself)
+RUN rosdep init && rosdep update && rosdep install --from-paths src -i -y --rosdistro noetic
+
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \
+    catkin init && \
+    catkin config --install -j 1 -p 1 && \
+    catkin build --limit-status-rate 0.1 --no-notify && \
+    catkin build --limit-status-rate 0.1 --no-notify --make-args tests"

imu_tools/imu_complementary_filter/CHANGELOG.rst

@@ -0,0 +1,100 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_complementary_filter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.2.3 (2021-04-09)
+------------------
+* Fix "non standard content" warning in imu_tools metapackage
+  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.2.2 (2020-05-25)
+------------------
+* fix install path & boost linkage issues
+* Contributors: Martin Günther, Sean Yen
+
+1.2.1 (2019-05-06)
+------------------
+* Remove junk xml (`#93 <https://github.com/ccny-ros-pkg/imu_tools/issues/93>`_)
+* Fix C++14 builds (`#89 <https://github.com/ccny-ros-pkg/imu_tools/issues/89>`_)
+* Contributors: David V. Lu!!, Paul Bovbel
+
+1.2.0 (2018-05-25)
+------------------
+* Add std dev parameter to orientation estimate from filter (`#85 <https://github.com/ccny-ros-pkg/imu_tools/issues/85>`_)
+  Similar to `#41 <https://github.com/ccny-ros-pkg/imu_tools/issues/41>`_, but not using dynamic_reconfigure as not implemented for complementary filter
+* Contributors: Stefan Kohlbrecher
+
+1.1.5 (2017-05-24)
+------------------
+
+1.1.4 (2017-05-22)
+------------------
+
+1.1.3 (2017-03-10)
+------------------
+* complementary_filter: move const initializations out of header
+  Initialization of static consts other than int (here: float) inside the
+  class declaration is not permitted in C++. It works in gcc (due to a
+  non-standard extension), but throws an error in C++11.
+* Contributors: Martin Guenther
+
+1.1.2 (2016-09-07)
+------------------
+
+1.1.1 (2016-09-07)
+------------------
+
+1.1.0 (2016-04-25)
+------------------
+
+1.0.11 (2016-04-22)
+-------------------
+
+1.0.10 (2016-04-22)
+-------------------
+* Remove Eigen dependency
+  Eigen is not actually used anywhere. Thanks @asimay!
+* Removed main function from shared library
+* Contributors: Martin Guenther, Matthias Nieuwenhuisen
+
+1.0.9 (2015-10-16)
+------------------
+* complementary: Add Eigen dependency
+  Fixes `#54 <https://github.com/ccny-ros-pkg/imu_tools/issues/54>`_.
+* Contributors: Martin Günther
+
+1.0.8 (2015-10-07)
+------------------
+
+1.0.7 (2015-10-07)
+------------------
+* Allow remapping imu namespace
+* Publish RPY as Vector3Stamped
+* Add params: constant_dt, publish_tf, reverse_tf, publish_debug_topics
+* Use MagneticField instead of Vector3
+* Contributors: Martin Günther
+
+1.0.6 (2015-10-06)
+------------------
+* Add new package: imu_complementary_filter
+* Contributors: Roberto G. Valentini, Martin Günther, Michael Görner
+
+1.0.5 (2015-06-24)
+------------------
+
+1.0.4 (2015-05-06)
+------------------
+
+1.0.3 (2015-01-29)
+------------------
+
+1.0.2 (2015-01-27)
+------------------
+
+1.0.1 (2014-12-10)
+------------------
+
+1.0.0 (2014-11-28)
+------------------

imu_tools/imu_complementary_filter/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(imu_complementary_filter)
+
+find_package(Boost REQUIRED COMPONENTS thread)
+
+find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  message_filters
+  roscpp
+  sensor_msgs
+  std_msgs
+  tf
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES complementary_filter
+  CATKIN_DEPENDS message_filters roscpp sensor_msgs std_msgs tf
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(complementary_filter
+  src/complementary_filter.cpp
+  src/complementary_filter_ros.cpp
+  include/imu_complementary_filter/complementary_filter.h
+  include/imu_complementary_filter/complementary_filter_ros.h
+)
+target_link_libraries(complementary_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+
+# create complementary_filter_node executable
+add_executable(complementary_filter_node
+  src/complementary_filter_node.cpp)
+target_link_libraries(complementary_filter_node complementary_filter ${catkin_LIBRARIES})
+
+install(TARGETS complementary_filter
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(TARGETS complementary_filter_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h

@@ -0,0 +1,180 @@
+/*
+  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+	@section LICENSE
+  Copyright (c) 2015, City University of New York
+  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+	All rights reserved.
+
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+     1. Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+     2. Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+     3. Neither the name of the City College of New York nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+
+	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+	ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+	WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+	DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
+	DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+	(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+	LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+	ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+	(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+	SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_H
+#define IMU_TOOLS_COMPLEMENTARY_FILTER_H
+
+namespace imu_tools {
+
+class ComplementaryFilter
+{
+  public:
+    ComplementaryFilter();    
+    virtual ~ComplementaryFilter();
+
+    bool setGainAcc(double gain);
+    bool setGainMag(double gain);
+    double getGainAcc() const;
+    double getGainMag() const;
+
+    bool setBiasAlpha(double bias_alpha);
+    double getBiasAlpha() const;
+
+    // When the filter is in the steady state, bias estimation will occur (if the
+    // parameter is enabled).
+    bool getSteadyState() const;
+
+    void setDoBiasEstimation(bool do_bias_estimation);
+    bool getDoBiasEstimation() const;
+
+    void setDoAdaptiveGain(bool do_adaptive_gain);
+    bool getDoAdaptiveGain() const;
+  
+    double getAngularVelocityBiasX() const;
+    double getAngularVelocityBiasY() const;
+    double getAngularVelocityBiasZ() const;
+
+    // Set the orientation, as a Hamilton Quaternion, of the body frame wrt the
+    // fixed frame.
+    void setOrientation(double q0, double q1, double q2, double q3);
+
+    // Get the orientation, as a Hamilton Quaternion, of the body frame wrt the
+    // fixed frame.
+    void getOrientation(double& q0, double& q1, double& q2, double& q3) const;
+
+    // Update from accelerometer and gyroscope data.
+    // [ax, ay, az]: Normalized gravity vector.
+    // [wx, wy, wz]: Angular veloctiy, in rad / s.
+    // dt: time delta, in seconds.
+    void update(double ax, double ay, double az, 
+                double wx, double wy, double wz,
+                double dt);
+
+    // Update from accelerometer, gyroscope, and magnetometer data.
+    // [ax, ay, az]: Normalized gravity vector.
+    // [wx, wy, wz]: Angular veloctiy, in rad / s.
+    // [mx, my, mz]: Magnetic field, units irrelevant.
+    // dt: time delta, in seconds.
+    void update(double ax, double ay, double az, 
+                double wx, double wy, double wz,
+                double mx, double my, double mz,
+                double dt);
+
+  private:
+    static const double kGravity;
+    static const double gamma_;
+    // Bias estimation steady state thresholds
+    static const double kAngularVelocityThreshold;
+    static const double kAccelerationThreshold;
+    static const double kDeltaAngularVelocityThreshold;
+
+    // Gain parameter for the complementary filter, belongs in [0, 1].
+    double gain_acc_;
+    double gain_mag_;
+
+    // Bias estimation gain parameter, belongs in [0, 1].
+    double bias_alpha_;
+
+    // Parameter whether to do bias estimation or not.
+    bool do_bias_estimation_;
+    
+    // Parameter whether to do adaptive gain or not.
+    bool do_adaptive_gain_;
+
+    bool initialized_;
+    bool steady_state_;
+
+    // The orientation as a Hamilton quaternion (q0 is the scalar). Represents
+    // the orientation of the fixed frame wrt the body frame.
+    double q0_, q1_, q2_, q3_; 
+
+    // Bias in angular velocities;
+    double wx_prev_, wy_prev_, wz_prev_;
+
+    // Bias in angular velocities;
+    double wx_bias_, wy_bias_, wz_bias_;
+
+    void updateBiases(double ax, double ay, double az, 
+                      double wx, double wy, double wz);
+
+    bool checkState(double ax, double ay, double az, 
+                    double wx, double wy, double wz) const;
+
+    void getPrediction(
+        double wx, double wy, double wz, double dt, 
+        double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const;
+   
+    void getMeasurement(
+        double ax, double ay, double az, 
+        double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
+
+    void getMeasurement(
+        double ax, double ay, double az, 
+        double mx, double my, double mz,  
+        double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
+
+    void getAccCorrection(
+        double ax, double ay, double az,
+        double p0, double p1, double p2, double p3,
+        double& dq0, double& dq1, double& dq2, double& dq3);
+   
+    void getMagCorrection(
+        double mx, double my, double mz,
+        double p0, double p1, double p2, double p3,
+        double& dq0, double& dq1, double& dq2, double& dq3); 
+    
+    double getAdaptiveGain(double alpha, double ax, double ay, double az);                   
+};
+
+// Utility math functions:
+
+void normalizeVector(double& x, double& y, double& z);
+
+void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3);
+
+void scaleQuaternion(double gain,
+                     double& dq0, double& dq1, double& dq2, double& dq3); 
+
+void invertQuaternion(
+    double q0, double q1, double q2, double q3,
+    double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv);
+
+void quaternionMultiplication(double p0, double p1, double p2, double p3,
+                              double q0, double q1, double q2, double q3,
+                              double& r0, double& r1, double& r2, double& r3);
+
+void rotateVectorByQuaternion(double x, double y, double z,
+                              double q0, double q1, double q2, double q3,
+                              double& vx, double& vy, double& vz);
+
+}  // namespace imu
+
+#endif  // IMU_TOOLS_COMPLEMENTARY_FILTER_H

imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h

@@ -0,0 +1,108 @@
+/*
+  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+	@section LICENSE
+  Copyright (c) 2015, City University of New York
+  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+	All rights reserved.
+
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+     1. Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+     2. Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+     3. Neither the name of the City College of New York nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+
+	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+	ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+	WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+	DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
+	DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+	(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+	LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+	ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+	(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+	SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H
+#define IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H
+
+#include <sensor_msgs/MagneticField.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_broadcaster.h>
+
+#include "imu_complementary_filter/complementary_filter.h"
+
+namespace imu_tools {
+
+class ComplementaryFilterROS
+{
+  public:
+    ComplementaryFilterROS(const ros::NodeHandle& nh, 
+                           const ros::NodeHandle& nh_private);    
+    virtual ~ComplementaryFilterROS();
+
+  private:
+
+    // Convenience typedefs
+    typedef sensor_msgs::Imu ImuMsg;
+    typedef sensor_msgs::MagneticField MagMsg;
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu, 
+        MagMsg> MySyncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> 
+        SyncPolicy;
+    typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;    
+    typedef message_filters::Subscriber<ImuMsg> ImuSubscriber; 
+    typedef message_filters::Subscriber<MagMsg> MagSubscriber;
+
+    // ROS-related variables.
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_private_;
+    
+    boost::shared_ptr<Synchronizer> sync_;
+    boost::shared_ptr<ImuSubscriber> imu_subscriber_;
+    boost::shared_ptr<MagSubscriber> mag_subscriber_;
+
+    ros::Publisher imu_publisher_;
+    ros::Publisher rpy_publisher_;
+    ros::Publisher state_publisher_;
+    tf::TransformBroadcaster tf_broadcaster_;
+         
+    // Parameters:
+    bool use_mag_;
+    bool publish_tf_;
+    bool reverse_tf_;
+    double constant_dt_;
+    bool publish_debug_topics_;
+    std::string fixed_frame_;
+    double orientation_variance_;
+
+    // State variables:
+    ComplementaryFilter filter_;
+    ros::Time time_prev_;
+    bool initialized_filter_;
+
+    void initializeParams();
+    void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw);
+    void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
+                        const MagMsg::ConstPtr& mav_msg);
+    void publish(const sensor_msgs::Imu::ConstPtr& imu_msg_raw);
+
+    tf::Quaternion hamiltonToTFQuaternion(
+        double q0, double q1, double q2, double q3) const;
+};
+
+}  // namespace imu_tools
+
+#endif // IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H

imu_tools/imu_complementary_filter/launch/complementary_filter.launch

@@ -0,0 +1,34 @@
+<!-- ComplementaryFilter launch file -->
+<launch>
+  #### Nodelet manager ######################################################
+
+  <node pkg="nodelet" type="nodelet" name="imu_manager" 
+    args="manager" output="screen" />
+
+  #### IMU Driver ###########################################################
+
+  <node pkg="nodelet" type="nodelet" name="PhidgetsImuNodelet" 
+    args="load phidgets_imu/PhidgetsImuNodelet imu_manager" 
+    output="screen">
+
+    # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms)
+    <param name="period" value="4"/>
+
+  </node>
+
+  #### Complementary filter
+
+  <node pkg="imu_complementary_filter" type="complementary_filter_node"
+      name="complementary_filter_gain_node" output="screen">
+    <param name="do_bias_estimation" value="true"/>
+    <param name="do_adaptive_gain" value="true"/>
+    <param name="use_mag" value="false"/>
+    <param name="gain_acc" value="0.01"/>
+    <param name="gain_mag" value="0.01"/>
+ 
+  
+  </node>
+  
+
+
+</launch>

imu_tools/imu_complementary_filter/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package>
+  <name>imu_complementary_filter</name>
+  <version>1.2.3</version>
+  <description>Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into a quaternion to represent the orientation of the device wrt the global frame. Based on the algorithm by Roberto G. Valenti etal. described in the paper "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" available at http://www.mdpi.com/1424-8220/15/8/19302 .</description>
+
+  <maintainer email="robertogl.valenti@gmail.com">Roberto G. Valenti</maintainer>
+  <license>BSD</license>
+ 
+  <url>http://www.mdpi.com/1424-8220/15/8/19302</url>
+  <author email="robertogl.valenti@gmail.com">Roberto G. Valenti</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf</run_depend>
+</package>

imu_tools/imu_complementary_filter/src/complementary_filter.cpp

@@ -0,0 +1,551 @@
+/*
+  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+	@section LICENSE
+  Copyright (c) 2015, City University of New York
+  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+	All rights reserved.
+
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+     1. Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+     2. Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+     3. Neither the name of the City College of New York nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+
+	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+	ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+	WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+	DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
+	DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+	(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+	LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+	ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+	(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+	SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "imu_complementary_filter/complementary_filter.h"
+
+#include <cstdio>
+#include <cmath>
+#include <iostream>
+
+namespace imu_tools {
+
+const double ComplementaryFilter::kGravity = 9.81;
+const double ComplementaryFilter::gamma_ = 0.01;
+// Bias estimation steady state thresholds
+const double ComplementaryFilter::kAngularVelocityThreshold = 0.2;
+const double ComplementaryFilter::kAccelerationThreshold = 0.1;
+const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01;
+
+ComplementaryFilter::ComplementaryFilter() :
+    gain_acc_(0.01),
+    gain_mag_(0.01),
+    bias_alpha_(0.01),
+    do_bias_estimation_(true),
+    do_adaptive_gain_(false),
+    initialized_(false),
+    steady_state_(false),
+    q0_(1), q1_(0), q2_(0), q3_(0),
+    wx_prev_(0), wy_prev_(0), wz_prev_(0),
+    wx_bias_(0), wy_bias_(0), wz_bias_(0) { }
+
+ComplementaryFilter::~ComplementaryFilter() { }
+
+void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
+{
+  do_bias_estimation_ = do_bias_estimation;
+}
+
+bool ComplementaryFilter::getDoBiasEstimation() const
+{
+  return do_bias_estimation_;
+}
+
+void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
+{
+  do_adaptive_gain_ = do_adaptive_gain;
+}
+
+bool ComplementaryFilter::getDoAdaptiveGain() const
+{
+  return do_adaptive_gain_;
+}
+
+bool ComplementaryFilter::setGainAcc(double gain)
+{
+  if (gain >= 0 && gain <= 1.0)
+  {
+    gain_acc_ = gain;
+    return true;
+  }
+  else
+    return false;
+}
+bool ComplementaryFilter::setGainMag(double gain)
+{
+  if (gain >= 0 && gain <= 1.0)
+  {
+    gain_mag_ = gain;
+    return true;
+  }
+  else
+    return false;
+}
+
+double ComplementaryFilter::getGainAcc() const 
+{
+  return gain_acc_;
+}
+
+double ComplementaryFilter::getGainMag() const 
+{
+  return gain_mag_;
+}
+
+bool ComplementaryFilter::getSteadyState() const 
+{
+  return steady_state_;
+}
+
+bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
+{
+  if (bias_alpha >= 0 && bias_alpha <= 1.0)
+  {
+    bias_alpha_ = bias_alpha;
+    return true;
+  }
+  else
+    return false;
+}
+
+double ComplementaryFilter::getBiasAlpha() const 
+{
+  return bias_alpha_;
+}
+
+void ComplementaryFilter::setOrientation(
+    double q0, double q1, double q2, double q3) 
+{
+  // Set the state to inverse (state is fixed wrt body).
+  invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_);
+}
+
+
+double ComplementaryFilter::getAngularVelocityBiasX() const
+{
+  return wx_bias_;
+}
+
+double ComplementaryFilter::getAngularVelocityBiasY() const
+{
+  return wy_bias_;
+}
+
+double ComplementaryFilter::getAngularVelocityBiasZ() const
+{
+  return wz_bias_;
+}
+
+void ComplementaryFilter::update(double ax, double ay, double az, 
+                                 double wx, double wy, double wz,
+                                 double dt)
+{
+  if (!initialized_) 
+  {
+    // First time - ignore prediction:
+    getMeasurement(ax, ay, az,
+                   q0_, q1_, q2_, q3_);
+    initialized_ = true;
+    return;
+  }
+  
+  // Bias estimation.
+  if (do_bias_estimation_)
+    updateBiases(ax, ay, az, wx, wy, wz);
+
+  // Prediction.
+  double q0_pred, q1_pred, q2_pred, q3_pred;
+  getPrediction(wx, wy, wz, dt,
+                q0_pred, q1_pred, q2_pred, q3_pred);   
+     
+  // Correction (from acc): 
+  // q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
+  // where qI = identity quaternion
+  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;  
+  getAccCorrection(ax, ay, az,
+                   q0_pred, q1_pred, q2_pred, q3_pred,
+                   dq0_acc, dq1_acc, dq2_acc, dq3_acc);
+  
+  double gain;
+  if (do_adaptive_gain_)
+  {  
+    gain = getAdaptiveGain(gain_acc_, ax, ay, az);
+    
+  }
+  else
+  {
+    gain = gain_acc_;
+    
+  }
+
+  scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
+
+  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
+                           dq0_acc, dq1_acc, dq2_acc, dq3_acc,
+                           q0_, q1_, q2_, q3_);
+
+  normalizeQuaternion(q0_, q1_, q2_, q3_);
+}
+
+void ComplementaryFilter::update(double ax, double ay, double az, 
+                                 double wx, double wy, double wz,
+                                 double mx, double my, double mz,
+                                 double dt)
+{
+  if (!initialized_) 
+  {
+    // First time - ignore prediction:
+    getMeasurement(ax, ay, az,
+                   mx, my, mz,
+                   q0_, q1_, q2_, q3_);
+    initialized_ = true;
+    return;
+  }
+
+  // Bias estimation.
+  if (do_bias_estimation_)
+    updateBiases(ax, ay, az, wx, wy, wz);
+
+  // Prediction.
+  double q0_pred, q1_pred, q2_pred, q3_pred;
+  getPrediction(wx, wy, wz, dt,
+                q0_pred, q1_pred, q2_pred, q3_pred);   
+     
+  // Correction (from acc): 
+  // q_temp = q_pred * [(1-gain) * qI + gain * dq_acc]
+  // where qI = identity quaternion
+  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;  
+  getAccCorrection(ax, ay, az,
+                   q0_pred, q1_pred, q2_pred, q3_pred,
+                   dq0_acc, dq1_acc, dq2_acc, dq3_acc);
+  double alpha = gain_acc_;  
+  if (do_adaptive_gain_)
+     alpha = getAdaptiveGain(gain_acc_, ax, ay, az);
+  scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
+
+  double q0_temp, q1_temp, q2_temp, q3_temp;
+  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
+                           dq0_acc, dq1_acc, dq2_acc, dq3_acc,
+                           q0_temp, q1_temp, q2_temp, q3_temp);
+  
+  // Correction (from mag):
+  // q_ = q_temp * [(1-gain) * qI + gain * dq_mag]
+  // where qI = identity quaternion
+  double dq0_mag, dq1_mag, dq2_mag, dq3_mag;  
+  getMagCorrection(mx, my, mz,
+                   q0_temp, q1_temp, q2_temp, q3_temp,
+                   dq0_mag, dq1_mag, dq2_mag, dq3_mag);
+
+  scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag);
+
+  quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp,
+                           dq0_mag, dq1_mag, dq2_mag, dq3_mag,
+                           q0_, q1_, q2_, q3_);
+
+  normalizeQuaternion(q0_, q1_, q2_, q3_);
+}
+
+bool ComplementaryFilter::checkState(double ax, double ay, double az, 
+                                     double wx, double wy, double wz) const
+{
+  double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
+  if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold)
+    return false;
+
+  if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
+      fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold ||
+      fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold)
+    return false;
+
+  if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
+      fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
+      fabs(wz - wz_bias_) > kAngularVelocityThreshold)
+    return false;
+
+  return true;
+}
+
+void ComplementaryFilter::updateBiases(double ax, double ay, double az, 
+                                       double wx, double wy, double wz)
+{
+  steady_state_ = checkState(ax, ay, az, wx, wy, wz);
+
+  if (steady_state_)
+  {
+    wx_bias_ += bias_alpha_ * (wx - wx_bias_);
+    wy_bias_ += bias_alpha_ * (wy - wy_bias_);
+    wz_bias_ += bias_alpha_ * (wz - wz_bias_);
+  }
+
+  wx_prev_ = wx; 
+  wy_prev_ = wy; 
+  wz_prev_ = wz;
+}
+
+void ComplementaryFilter::getPrediction(
+    double wx, double wy, double wz, double dt, 
+    double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
+{
+  double wx_unb = wx - wx_bias_;
+  double wy_unb = wy - wy_bias_;
+  double wz_unb = wz - wz_bias_;
+
+  q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
+  q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
+  q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
+  q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
+  
+  normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
+}
+
+void ComplementaryFilter::getMeasurement(
+    double ax, double ay, double az, 
+    double mx, double my, double mz,  
+    double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
+{
+  // q_acc is the quaternion obtained from the acceleration vector representing 
+  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
+  // (intermediary frame). q3_acc is defined as 0.
+  double q0_acc, q1_acc, q2_acc, q3_acc;
+    
+  // Normalize acceleration vector.
+  normalizeVector(ax, ay, az);
+  if (az >=0)
+    {
+      q0_acc =  sqrt((az + 1) * 0.5);	
+      q1_acc = -ay/(2.0 * q0_acc);
+      q2_acc =  ax/(2.0 * q0_acc);
+      q3_acc = 0;
+    }
+    else 
+    {
+      double X = sqrt((1 - az) * 0.5);
+      q0_acc = -ay/(2.0 * X);
+      q1_acc = X;
+      q2_acc = 0;
+      q3_acc = ax/(2.0 * X);
+    }  
+  
+  // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
+  // frame by the inverse of q_acc.
+  // l = R(q_acc)^-1 m
+  double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx + 
+      2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
+  double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc + 
+      q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
+  
+  // q_mag is the quaternion that rotates the Global frame (North West Up) into
+  // the intermediary frame. q1_mag and q2_mag are defined as 0.
+	double gamma = lx*lx + ly*ly;	
+	double beta = sqrt(gamma + lx*sqrt(gamma));
+  double q0_mag = beta / (sqrt(2.0 * gamma));  
+  double q3_mag = ly / (sqrt(2.0) * beta); 
+    
+  // The quaternion multiplication between q_acc and q_mag represents the 
+  // quaternion, orientation of the Global frame wrt the local frame.  
+  // q = q_acc times q_mag 
+  quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, 
+                           q0_mag, 0, 0, q3_mag,
+                           q0_meas, q1_meas, q2_meas, q3_meas ); 
+  //q0_meas = q0_acc*q0_mag;     
+  //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
+  //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
+  //q3_meas = q0_acc*q3_mag;
+}
+
+
+void ComplementaryFilter::getMeasurement(
+    double ax, double ay, double az, 
+    double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
+{
+  // q_acc is the quaternion obtained from the acceleration vector representing 
+  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
+  // (intermediary frame). q3_acc is defined as 0.
+     
+  // Normalize acceleration vector.
+  normalizeVector(ax, ay, az);
+
+  if (az >=0)
+  {
+    q0_meas =  sqrt((az + 1) * 0.5);	
+    q1_meas = -ay/(2.0 * q0_meas);
+    q2_meas =  ax/(2.0 * q0_meas);
+    q3_meas = 0;
+  }
+  else 
+  {
+    double X = sqrt((1 - az) * 0.5);
+    q0_meas = -ay/(2.0 * X);
+    q1_meas = X;
+    q2_meas = 0;
+    q3_meas = ax/(2.0 * X);
+  }  
+}
+
+void ComplementaryFilter::getAccCorrection(
+  double ax, double ay, double az,
+  double p0, double p1, double p2, double p3,
+  double& dq0, double& dq1, double& dq2, double& dq3)
+{
+  // Normalize acceleration vector.
+  normalizeVector(ax, ay, az);
+  
+  // Acceleration reading rotated into the world frame by the inverse predicted
+  // quaternion (predicted gravity):
+  double gx, gy, gz;
+  rotateVectorByQuaternion(ax, ay, az,
+                           p0, -p1, -p2, -p3, 
+                           gx, gy, gz);
+  
+  // Delta quaternion that rotates the predicted gravity into the real gravity:
+  dq0 =  sqrt((gz + 1) * 0.5);	
+  dq1 = -gy/(2.0 * dq0);
+  dq2 =  gx/(2.0 * dq0);
+  dq3 =  0.0;
+}
+
+void ComplementaryFilter::getMagCorrection(
+  double mx, double my, double mz,
+  double p0, double p1, double p2, double p3,
+  double& dq0, double& dq1, double& dq2, double& dq3)
+{
+  // Magnetic reading rotated into the world frame by the inverse predicted
+  // quaternion:
+  double lx, ly, lz;
+  rotateVectorByQuaternion(mx, my, mz,
+                           p0, -p1, -p2, -p3, 
+                           lx, ly, lz);
+   
+  // Delta quaternion that rotates the l so that it lies in the xz-plane 
+  // (points north):
+  double gamma = lx*lx + ly*ly;	
+	double beta = sqrt(gamma + lx*sqrt(gamma));
+  dq0 = beta / (sqrt(2.0 * gamma));
+  dq1 = 0.0;
+  dq2 = 0.0;  
+  dq3 = ly / (sqrt(2.0) * beta);  
+}
+ 
+void ComplementaryFilter::getOrientation(
+    double& q0, double& q1, double& q2, double& q3) const
+{
+  // Return the inverse of the state (state is fixed wrt body).
+  invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3);
+}
+
+double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az)
+{
+  double a_mag = sqrt(ax*ax + ay*ay + az*az);
+  double error = fabs(a_mag - kGravity)/kGravity;
+  double factor;
+  double error1 = 0.1;
+  double error2 = 0.2;
+  double m = 1.0/(error1 - error2);
+  double b = 1.0 - m*error1;
+  if (error < error1)
+    factor = 1.0;
+  else if (error < error2)
+    factor = m*error + b;
+  else 
+    factor = 0.0;
+  //printf("FACTOR: %f \n", factor);
+  return factor*alpha;
+}
+
+void normalizeVector(double& x, double& y, double& z)
+{
+  double norm = sqrt(x*x + y*y + z*z);
+
+  x /= norm;
+  y /= norm;
+  z /= norm;
+}
+
+void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
+{
+  double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+  q0 /= norm;  
+  q1 /= norm;
+  q2 /= norm;
+  q3 /= norm;
+}
+
+void invertQuaternion(
+  double q0, double q1, double q2, double q3,
+  double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv)
+{
+  // Assumes quaternion is normalized.
+  q0_inv = q0;
+  q1_inv = -q1;
+  q2_inv = -q2;
+  q3_inv = -q3;
+}
+
+void scaleQuaternion(
+  double gain,
+  double& dq0, double& dq1, double& dq2, double& dq3)
+{
+	if (dq0 < 0.0)//0.9
+  {
+    // Slerp (Spherical linear interpolation):
+    double angle = acos(dq0);
+    double A = sin(angle*(1.0 - gain))/sin(angle);
+    double B = sin(angle * gain)/sin(angle);
+    dq0 = A + B * dq0;
+    dq1 = B * dq1;
+    dq2 = B * dq2;
+    dq3 = B * dq3;
+  }
+  else
+  {
+    // Lerp (Linear interpolation):
+    dq0 = (1.0 - gain) + gain * dq0;
+    dq1 = gain * dq1;
+    dq2 = gain * dq2;
+    dq3 = gain * dq3;
+  }
+
+  normalizeQuaternion(dq0, dq1, dq2, dq3);  
+}
+
+void quaternionMultiplication(
+  double p0, double p1, double p2, double p3,
+  double q0, double q1, double q2, double q3,
+  double& r0, double& r1, double& r2, double& r3)
+{
+  // r = p q
+  r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
+  r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
+  r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
+  r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
+}
+
+void rotateVectorByQuaternion( 
+  double x, double y, double z,
+  double q0, double q1, double q2, double q3,
+  double& vx, double& vy, double& vz)
+{ 
+  vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
+  vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
+  vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
+}
+
+
+}  // namespace imu_tools

imu_tools/imu_complementary_filter/src/complementary_filter_node.cpp

@@ -0,0 +1,42 @@
+/*
+  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+	@section LICENSE
+  Copyright (c) 2015, City University of New York
+  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+	All rights reserved.
+
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+     1. Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+     2. Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+     3. Neither the name of the City College of New York nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+
+	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+	ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+	WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+	DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
+	DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+	(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+	LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+	ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+	(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+	SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "imu_complementary_filter/complementary_filter_ros.h"
+
+int main (int argc, char **argv)
+{
+  ros::init (argc, argv, "ComplementaryFilterROS");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_private("~");
+  imu_tools::ComplementaryFilterROS filter(nh, nh_private);
+  ros::spin();
+  return 0;
+}

imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp

@@ -0,0 +1,305 @@
+/*
+  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+	@section LICENSE
+  Copyright (c) 2015, City University of New York
+  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+	All rights reserved.
+
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+     1. Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+     2. Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+     3. Neither the name of the City College of New York nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+
+	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+	ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+	WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+	DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
+	DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+	(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+	LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+	ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+	(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+	SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "imu_complementary_filter/complementary_filter_ros.h"
+
+#include <std_msgs/Float64.h>
+#include <std_msgs/Bool.h>
+
+namespace imu_tools {
+
+ComplementaryFilterROS::ComplementaryFilterROS(
+    const ros::NodeHandle& nh,
+    const ros::NodeHandle& nh_private):
+  nh_(nh),
+  nh_private_(nh_private),
+  initialized_filter_(false)
+{
+  ROS_INFO("Starting ComplementaryFilterROS");
+  initializeParams();
+
+  int queue_size = 5;
+
+  // Register publishers:
+  imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(ros::names::resolve("imu") + "/data", queue_size);
+
+  if (publish_debug_topics_)
+  {
+      rpy_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
+                  ros::names::resolve("imu") + "/rpy/filtered", queue_size);
+
+      if (filter_.getDoBiasEstimation())
+      {
+        state_publisher_ = nh_.advertise<std_msgs::Bool>(
+                    ros::names::resolve("imu") + "/steady_state", queue_size);
+      }
+  }
+
+  // Register IMU raw data subscriber.
+  imu_subscriber_.reset(new ImuSubscriber(nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
+
+  // Register magnetic data subscriber.
+  if (use_mag_)
+  {
+    mag_subscriber_.reset(new MagSubscriber(nh_, ros::names::resolve("imu") + "/mag", queue_size));
+
+    sync_.reset(new Synchronizer(
+        SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
+    sync_->registerCallback(
+        boost::bind(&ComplementaryFilterROS::imuMagCallback, this, _1, _2));
+  }
+  else
+  {
+    imu_subscriber_->registerCallback(
+        &ComplementaryFilterROS::imuCallback, this);
+  }
+}
+
+ComplementaryFilterROS::~ComplementaryFilterROS()
+{
+  ROS_INFO("Destroying ComplementaryFilterROS");
+}
+
+void ComplementaryFilterROS::initializeParams()
+{
+  double gain_acc;
+  double gain_mag;
+  bool do_bias_estimation;
+  double bias_alpha;
+  bool do_adaptive_gain;
+
+  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
+    fixed_frame_ = "odom";
+  if (!nh_private_.getParam ("use_mag", use_mag_))
+    use_mag_ = false;
+  if (!nh_private_.getParam ("publish_tf", publish_tf_))
+    publish_tf_ = false;
+  if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
+    reverse_tf_ = false;
+  if (!nh_private_.getParam ("constant_dt", constant_dt_))
+    constant_dt_ = 0.0;
+  if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
+    publish_debug_topics_ = false;
+  if (!nh_private_.getParam ("gain_acc", gain_acc))
+    gain_acc = 0.01;
+  if (!nh_private_.getParam ("gain_mag", gain_mag))
+    gain_mag = 0.01;
+  if (!nh_private_.getParam ("do_bias_estimation", do_bias_estimation))
+    do_bias_estimation = true;
+  if (!nh_private_.getParam ("bias_alpha", bias_alpha))
+    bias_alpha = 0.01;
+  if (!nh_private_.getParam ("do_adaptive_gain", do_adaptive_gain))
+    do_adaptive_gain = true;
+
+  double orientation_stddev;
+  if (!nh_private_.getParam ("orientation_stddev", orientation_stddev))
+    orientation_stddev = 0.0;
+
+  orientation_variance_ = orientation_stddev * orientation_stddev;
+
+  filter_.setDoBiasEstimation(do_bias_estimation);
+  filter_.setDoAdaptiveGain(do_adaptive_gain);
+
+  if(!filter_.setGainAcc(gain_acc))
+    ROS_WARN("Invalid gain_acc passed to ComplementaryFilter.");
+  if (use_mag_)
+  {
+    if(!filter_.setGainMag(gain_mag))
+      ROS_WARN("Invalid gain_mag passed to ComplementaryFilter.");
+  }
+  if (do_bias_estimation)
+  {
+    if(!filter_.setBiasAlpha(bias_alpha))
+      ROS_WARN("Invalid bias_alpha passed to ComplementaryFilter.");
+  }
+
+  // check for illegal constant_dt values
+  if (constant_dt_ < 0.0)
+  {
+    // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
+    // otherwise, it will be constant
+    ROS_WARN("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
+    constant_dt_ = 0.0;
+  }
+}
+
+void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
+{
+  const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
+  const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
+  const ros::Time& time = imu_msg_raw->header.stamp;
+
+  // Initialize.
+  if (!initialized_filter_)
+  {
+    time_prev_ = time;
+    initialized_filter_ = true;
+    return;
+  }
+
+  // determine dt: either constant, or from IMU timestamp
+  double dt;
+  if (constant_dt_ > 0.0)
+    dt = constant_dt_;
+  else
+    dt = (time - time_prev_).toSec();
+
+  time_prev_ = time;
+
+  // Update the filter.
+  filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
+
+  // Publish state.
+  publish(imu_msg_raw);
+}
+
+void ComplementaryFilterROS::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
+                                            const MagMsg::ConstPtr& mag_msg)
+{
+  const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
+  const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
+  const geometry_msgs::Vector3& m = mag_msg->magnetic_field;
+  const ros::Time& time = imu_msg_raw->header.stamp;
+
+  // Initialize.
+  if (!initialized_filter_)
+  {
+    time_prev_ = time;
+    initialized_filter_ = true;
+    return;
+  }
+
+  // Calculate dt.
+  double dt = (time - time_prev_).toSec();
+  time_prev_ = time;
+   //ros::Time t_in, t_out;
+  //t_in = ros::Time::now();
+  // Update the filter.
+  if (std::isnan(m.x) || std::isnan(m.y) || std::isnan(m.z))
+    filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
+  else
+    filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, m.x, m.y, m.z, dt);
+
+  //t_out = ros::Time::now();
+  //float dt_tot = (t_out - t_in).toSec() * 1000.0; // In msec.
+  //printf("%.6f\n", dt_tot);
+  // Publish state.
+  publish(imu_msg_raw);
+}
+
+tf::Quaternion ComplementaryFilterROS::hamiltonToTFQuaternion(
+    double q0, double q1, double q2, double q3) const
+{
+  // ROS uses the Hamilton quaternion convention (q0 is the scalar). However,
+  // the ROS quaternion is in the form [x, y, z, w], with w as the scalar.
+  return tf::Quaternion(q1, q2, q3, q0);
+}
+
+void ComplementaryFilterROS::publish(
+    const sensor_msgs::Imu::ConstPtr& imu_msg_raw)
+{
+  // Get the orientation:
+  double q0, q1, q2, q3;
+  filter_.getOrientation(q0, q1, q2, q3);
+  tf::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3);
+
+  // Create and publish fitlered IMU message.
+  boost::shared_ptr<sensor_msgs::Imu> imu_msg =
+      boost::make_shared<sensor_msgs::Imu>(*imu_msg_raw);
+  tf::quaternionTFToMsg(q, imu_msg->orientation);
+
+  imu_msg->orientation_covariance[0] = orientation_variance_;
+  imu_msg->orientation_covariance[1] = 0.0;
+  imu_msg->orientation_covariance[2] = 0.0;
+  imu_msg->orientation_covariance[3] = 0.0;
+  imu_msg->orientation_covariance[4] = orientation_variance_;
+  imu_msg->orientation_covariance[5] = 0.0;
+  imu_msg->orientation_covariance[6] = 0.0;
+  imu_msg->orientation_covariance[7] = 0.0;
+  imu_msg->orientation_covariance[8] = orientation_variance_;
+
+  // Account for biases.
+  if (filter_.getDoBiasEstimation())
+  {
+    imu_msg->angular_velocity.x -= filter_.getAngularVelocityBiasX();
+    imu_msg->angular_velocity.y -= filter_.getAngularVelocityBiasY();
+    imu_msg->angular_velocity.z -= filter_.getAngularVelocityBiasZ();
+  }
+
+  imu_publisher_.publish(imu_msg);
+
+  if (publish_debug_topics_)
+  {
+      // Create and publish roll, pitch, yaw angles
+      geometry_msgs::Vector3Stamped rpy;
+      rpy.header = imu_msg_raw->header;
+
+      tf::Matrix3x3 M;
+      M.setRotation(q);
+      M.getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
+      rpy_publisher_.publish(rpy);
+
+      // Publish whether we are in the steady state, when doing bias estimation
+      if (filter_.getDoBiasEstimation())
+      {
+        std_msgs::Bool state_msg;
+        state_msg.data = filter_.getSteadyState();
+        state_publisher_.publish(state_msg);
+      }
+  }
+
+  if (publish_tf_)
+  {
+      // Create and publish the ROS tf.
+      tf::Transform transform;
+      transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
+      transform.setRotation(q);
+
+      if (reverse_tf_)
+      {
+          tf_broadcaster_.sendTransform(
+              tf::StampedTransform(transform.inverse(),
+                                   imu_msg_raw->header.stamp,
+                                   imu_msg_raw->header.frame_id,
+                                   fixed_frame_));
+      }
+      else
+      {
+          tf_broadcaster_.sendTransform(
+              tf::StampedTransform(transform,
+                                   imu_msg_raw->header.stamp,
+                                   fixed_frame_,
+                                   imu_msg_raw->header.frame_id));
+      }
+  }
+}
+
+}  // namespace imu_tools

imu_tools/imu_filter_madgwick/CHANGELOG.rst

@@ -0,0 +1,217 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_filter_madgwick
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.2.3 (2021-04-09)
+------------------
+* Fix "non standard content" warning in imu_tools metapackage
+  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
+* Add example launch file for imu_filter_madgwick (`#132 <https://github.com/ccny-ros-pkg/imu_tools/issues/132>`_)
+* Update maintainers in package.xml
+* Fix warnings: reordering and unused vars
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther, pietrocolombo
+
+1.2.2 (2020-05-25)
+------------------
+* Drop the signals component of Boost (`#103 <https://github.com/ccny-ros-pkg/imu_tools/issues/103>`_)
+* Add the option to remove the gravity vector (`#101 <https://github.com/ccny-ros-pkg/imu_tools/issues/101>`_)
+* fix install path & boost linkage issues
+* Contributors: Alexis Paques, Martin Günther, Mike Purvis, Sean Yen
+
+1.2.1 (2019-05-06)
+------------------
+* Skip messages and warn if computeOrientation fails
+* Contributors: Martin Günther
+
+1.2.0 (2018-05-25)
+------------------
+* Remove outdated Makefile
+* Add warning when IMU time stamp is zero
+  Closes `#82 <https://github.com/ccny-ros-pkg/imu_tools/issues/82>`_.
+* update to use non deprecated pluginlib macro (`#77 <https://github.com/ccny-ros-pkg/imu_tools/issues/77>`_)
+* Contributors: Martin Günther, Mikael Arguedas
+
+1.1.5 (2017-05-24)
+------------------
+* Initial release into Lunar
+* Remove support for Vector3 mag messages
+* Change default world_frame = enu
+* Rewrite rosbags: Use MagneticField for magnetometer
+* Contributors: Martin Günther
+
+1.1.4 (2017-05-22)
+------------------
+* Print warning if waiting for topic
+  Closes `#61 <https://github.com/ccny-ros-pkg/imu_tools/issues/61>`_.
+* Fix boost::lock_error on shutdown
+* Contributors: Martin Günther
+
+1.1.3 (2017-03-10)
+------------------
+* Return precisely normalized quaternions
+  Fixes `#67 <https://github.com/ccny-ros-pkg/imu_tools/issues/67>`_ : TF_DENORMALIZED_QUATERNION warning added in TF2 0.5.14.
+* Tests: Check that output quaternions are normalized
+* Fixed lock so it stays in scope until end of method.
+* Contributors: Jason Mercer, Martin Guenther, Martin Günther
+
+1.1.2 (2016-09-07)
+------------------
+* Add missing dependency on tf2_geometry_msgs
+* Contributors: Martin Guenther
+
+1.1.1 (2016-09-07)
+------------------
+* Add parameter "world_frame": optionally use ENU or NED instead of NWU
+  convention (from `#60 <https://github.com/ccny-ros-pkg/imu_tools/issues/60>`_;
+  closes `#36 <https://github.com/ccny-ros-pkg/imu_tools/issues/36>`_)
+* Add parameter "stateless" for debugging purposes: don't do any stateful
+  filtering, but instead publish the orientation directly computed from the
+  latest accelerometer (+ optionally magnetometer) readings alone
+* Replace the (buggy) Euler-angle-based initialization routine
+  (ImuFilterRos::computeRPY) by a correct transformation
+  matrix based one (StatelessOrientation::computeOrientation) and make it
+  available as a library function
+* Refactor madgwickAHRSupdate() (pull out some functions, remove micro
+  optimizations to improve readability)
+* Add unit tests
+* Contributors: Martin Guenther, Michael Stoll
+
+1.1.0 (2016-04-25)
+------------------
+
+1.0.11 (2016-04-22)
+-------------------
+* Jade: Change default: use_magnetic_field_msg = true
+* Contributors: Martin Guenther
+
+1.0.10 (2016-04-22)
+-------------------
+
+1.0.9 (2015-10-16)
+------------------
+
+1.0.8 (2015-10-07)
+------------------
+
+1.0.7 (2015-10-07)
+------------------
+
+1.0.6 (2015-10-06)
+------------------
+* Split ImuFilter class into ImuFilter and ImuFilterRos in order to
+  have a C++ API to the Madgwick algorithm
+* Properly install header files.
+* Contributors: Martin Günther, Michael Stoll
+
+1.0.5 (2015-06-24)
+------------------
+* Add "~use_magnetic_field_msg" param.
+  This allows the user to subscribe to the /imu/mag topic as a
+  sensor_msgs/MagneticField rather than a geometry_msgs/Vector3Stamped.
+  The default for now is false, which preserves the legacy behaviour via a
+  separate subscriber which converts Vector3Stamped to MagneticField and
+  republishes.
+* Contributors: Mike Purvis, Martin Günther
+
+1.0.4 (2015-05-06)
+------------------
+* update dynamic reconfigure param descriptions
+* only advertise debug topics if they are used
+* allow remapping of the whole imu namespace
+  with this change, all topics can be remapped at once, like this:
+  rosrun imu_filter_madgwick imu_filter_node imu:=my_imu
+* Contributors: Martin Günther
+
+1.0.3 (2015-01-29)
+------------------
+* Add std dev parameter to orientation estimate covariance matrix
+* Port imu_filter_madgwick to tf2
+* Switch to smart pointer
+* Contributors: Paul Bovbel, Martin Günther
+
+1.0.2 (2015-01-27)
+------------------
+* fix tf publishing (switch parent + child frames)
+  The orientation is between a fixed inertial frame (``fixed_frame_``) and
+  the frame that the IMU is mounted in (``imu_frame_``). Also,
+  ``imu_msg.header.frame`` should be ``imu_frame_``, but the corresponding TF
+  goes from ``fixed_frame_`` to ``imu_frame_``. This commit fixes that; for
+  the ``reverse_tf`` case, it was already correct.
+  Also see http://answers.ros.org/question/50870/what-frame-is-sensor_msgsimuorientation-relative-to/.
+  Note that tf publishing should be enabled for debug purposes only, since we can only
+  provide the orientation, not the translation.
+* Add ~reverse_tf parameter for the robots which does not have IMU on root-link
+* Log mag bias on startup to assist with debugging.
+* add boost depends to CMakeLists
+  All non-catkin things that we expose in our headers should be added to
+  the DEPENDS, so that packages which depend on our package will also
+  automatically link against it.
+* Contributors: Martin Günther, Mike Purvis, Ryohei Ueda
+
+1.0.1 (2014-12-10)
+------------------
+* add me as maintainer to package.xml
+* turn mag_bias into a dynamic reconfigure param
+  Also rename mag_bias/x --> mag_bias_x etc., since dynamic reconfigure
+  doesn't allow slashes.
+* gain and zeta already set via dynamic_reconfigure
+  Reading the params explicitly is not necessary. Instead,
+  dynamic_reconfigure will read them and set them as soon as we call
+  config_server->setCallback().
+* reconfigure server: use proper namespace
+  Before, the reconfigure server used the private namespace of the nodelet
+  *manager* instead of the nodelet, so the params on the parameter server
+  and the ones from dynamic_reconfigure were out of sync.
+* check for NaNs in magnetometer message
+  Some magnetometer drivers (e.g. phidgets_drivers) output NaNs, which
+  is a valid way of saying that this measurement is invalid. During
+  initialization, we simply wait for the first valid message, assuming
+  there will be one soon.
+* magnetometer msg check: isnan() -> !isfinite()
+  This catches both inf and NaN. Not sure whether sending inf in a Vector3
+  message is valid (Nan is), but this doesn't hurt and is just good
+  defensive programming.
+* Initialize yaw from calibrated magnetometer data
+  * Add magnetometer biases (mag_bias/x and mag_bias/y) for hard-iron compensation.
+  * Initialize yaw orientation from magnetometer reading.
+  * 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.
+* Contributors: Martin Günther, Shokoofeh Pourmehr
+
+1.0.0 (2014-09-03)
+------------------
+* First public release
+* Remove setting imu message frame to fixed/odom
+* CMakeLists: remove unnecessary link_directories, LIBRARY_OUTPUT_PATH
+* add missing build dependency on generated config
+  This removes a racing condition from the build process.
+* install nodelet xml file
+  Otherwise the nodelet can't be found
+* fix implementation of invSqrt()
+  The old invSqrt() implementation causes the estimate to diverge under
+  constant input. The problem was the line `long i = (long)&y;`, where 64
+  bits are read from a 32 bit number. Thanks to @tomas-c for spotting this
+  and pointing out the solution.
+* catkinization of imu_tools metapackage
+* fix typo: zeta -> ``zeta_``
+* fix initialization of initial rotation
+* gyro drift correction function added in MARG implementation
+* set "zeta" as a parameter for dynamic reconfigure in the .cfg file
+* add new test bag: phidgets_imu_upside_down
+* add parameter publish_tf
+  When the imu is used together with other packages, such as
+  robot_pose_ekf, publishing the transform often interferes with those
+  packages. This parameter allows to disable tf publishing.
+* add some sample imu data
+* more informative constant_dt message. Reverts to 0.0 on illegal param value
+* imu_filter_madgwick manifest now correctly lists the package as GPL license.
+* orientation is initialized from acceleration vector on first message received
+* added dynamic reconfigure for gain parameter. Added better messages about constant_dt param at startup
+* the tf published is now timestamped as the imu msg, and not as now(). Also added constant dt option for the imu+mag callback
+* fix the transform publish -- from the fixed frame to the frame of the imu
+* add a tf broadcaster with the orientation
+* as per PaulKemppi: added option to set constant dt
+* walchko: Needed to add namespace: std::isnan() and needed to add rosbuild_link_boost(imu_filter signals) to CMakeLists.txt
+* added sebastian's name and link to the manifest
+* renamed imu_filter to imu_filter_madgwick
+* Contributors: Ivan Dryanovski, Martin Günther, Mike Purvis, Sameer Parekh, TUG-DESTOP, Francisco Vina, Michael Görner, Paul Kemppi, Tomas Cerskus, Kevin Walchko

imu_tools/imu_filter_madgwick/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(imu_filter_madgwick)
+
+find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs tf2 tf2_geometry_msgs tf2_ros nodelet pluginlib message_filters dynamic_reconfigure)
+
+find_package(Boost REQUIRED COMPONENTS system thread)
+
+# Generate dynamic parameters
+generate_dynamic_reconfigure_options(cfg/ImuFilterMadgwick.cfg)
+
+
+catkin_package(
+  DEPENDS Boost
+  CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs tf2_ros tf2_geometry_msgs nodelet pluginlib message_filters dynamic_reconfigure
+  INCLUDE_DIRS
+  LIBRARIES imu_filter imu_filter_nodelet
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+
+# create imu_filter library
+add_library (imu_filter src/imu_filter.cpp  src/imu_filter_ros.cpp src/stateless_orientation.cpp)
+add_dependencies(imu_filter ${PROJECT_NAME}_gencfg)
+target_link_libraries(imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+# create imu_filter_nodelet library
+add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp)
+add_dependencies(imu_filter_nodelet ${PROJECT_NAME}_gencfg)
+target_link_libraries(imu_filter_nodelet imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+# create imu_filter_node executable
+add_executable(imu_filter_node src/imu_filter_node.cpp)
+add_dependencies(imu_filter_node ${PROJECT_NAME}_gencfg)
+target_link_libraries(imu_filter_node imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+
+install(TARGETS imu_filter_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS imu_filter imu_filter_nodelet
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h"
+)
+
+install(FILES imu_filter_nodelet.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(${PROJECT_NAME}-madgwick_test
+    test/stateless_orientation_test.cpp
+    test/madgwick_test.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}-madgwick_test
+    imu_filter
+    ${catkin_LIBRARIES}
+  )
+endif()

imu_tools/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg

@@ -0,0 +1,18 @@
+#! /usr/bin/env python
+# IMU Filter dynamic reconfigure
+
+PACKAGE='imu_filter_madgwick'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+                                                                    
+gen.add("gain", double_t, 0, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal.", 0.1, 0.0, 1.0) 
+gen.add("zeta", double_t, 0, "Gyro drift gain (approx. rad/s).", 0, -1.0, 1.0) 
+gen.add("mag_bias_x", double_t, 0, "Magnetometer bias (hard iron correction), x component.", 0, -10.0, 10.0)
+gen.add("mag_bias_y", double_t, 0, "Magnetometer bias (hard iron correction), y component.", 0, -10.0, 10.0)
+gen.add("mag_bias_z", double_t, 0, "Magnetometer bias (hard iron correction), z component.", 0, -10.0, 10.0)
+gen.add("orientation_stddev", double_t, 0, "Standard deviation of the orientation estimate.", 0, 0, 1.0)
+
+exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "ImuFilterMadgwick"))
+

imu_tools/imu_filter_madgwick/imu_filter_nodelet.xml

@@ -0,0 +1,9 @@
+<!-- Imu Filter nodelet publisher -->
+<library path="lib/libimu_filter_nodelet">
+  <class name="imu_filter_madgwick/ImuFilterNodelet" type="ImuFilterNodelet" 
+    base_class_type="nodelet::Nodelet">
+    <description>
+      Imu Filter nodelet publisher.
+    </description>
+  </class>
+</library>

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

@@ -0,0 +1,107 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMU_FILTER_MADWICK_IMU_FILTER_H
+#define IMU_FILTER_MADWICK_IMU_FILTER_H
+
+#include <imu_filter_madgwick/world_frame.h>
+#include <iostream>
+#include <cmath>
+
+class ImuFilter
+{
+  public:
+
+    ImuFilter();
+    virtual ~ImuFilter();
+
+  private:
+    // **** paramaters
+    double gain_;    // algorithm gain
+    double zeta_;    // gyro drift bias gain
+    WorldFrame::WorldFrame world_frame_;    // NWU, ENU, NED
+
+    // **** state variables
+    double q0, q1, q2, q3;  // quaternion
+    float w_bx_, w_by_, w_bz_; //
+
+public:
+    void setAlgorithmGain(double gain)
+    {
+        gain_ = gain;
+    }
+
+    void setDriftBiasGain(double zeta)
+    {
+        zeta_ = zeta;
+    }
+
+    void setWorldFrame(WorldFrame::WorldFrame frame)
+    {
+        world_frame_ = frame;
+    }
+
+    void getOrientation(double& q0, double& q1, double& q2, double& q3)
+    {
+        q0 = this->q0;
+        q1 = this->q1;
+        q2 = this->q2;
+        q3 = this->q3;
+
+        // perform precise normalization of the output, using 1/sqrt()
+        // instead of the fast invSqrt() approximation. Without this,
+        // TF2 complains that the quaternion is not normalized.
+        double recipNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+        q0 *= recipNorm;
+        q1 *= recipNorm;
+        q2 *= recipNorm;
+        q3 *= recipNorm;
+    }
+
+    void setOrientation(double q0, double q1, double q2, double q3)
+    {
+        this->q0 = q0;
+        this->q1 = q1;
+        this->q2 = q2;
+        this->q3 = q3;
+
+        w_bx_ = 0;
+        w_by_ = 0;
+        w_bz_ = 0;
+    }
+
+    void madgwickAHRSupdate(float gx, float gy, float gz,
+                            float ax, float ay, float az,
+                            float mx, float my, float mz,
+                            float dt);
+
+    void madgwickAHRSupdateIMU(float gx, float gy, float gz,
+                               float ax, float ay, float az,
+                               float dt);
+
+    void getGravity(float& rx, float& ry, float& rz,
+                    float gravity = 9.80665);
+};
+
+#endif // IMU_FILTER_IMU_MADWICK_FILTER_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_nodelet.h

@@ -0,0 +1,41 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H
+#define IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H
+
+#include <nodelet/nodelet.h>
+
+#include "imu_filter_madgwick/imu_filter_ros.h"
+
+class ImuFilterNodelet : public nodelet::Nodelet
+{
+  public:
+    virtual void onInit();
+
+  private:
+    boost::shared_ptr<ImuFilterRos> filter_;
+};
+
+#endif // IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

@@ -0,0 +1,116 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMU_FILTER_MADWICK_IMU_FILTER_ROS_H
+#define IMU_FILTER_MADWICK_IMU_FILTER_ROS_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/MagneticField.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include "tf2_ros/transform_broadcaster.h"
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <dynamic_reconfigure/server.h>
+
+#include "imu_filter_madgwick/imu_filter.h"
+#include "imu_filter_madgwick/ImuFilterMadgwickConfig.h"
+
+class ImuFilterRos
+{
+  typedef sensor_msgs::Imu              ImuMsg;
+  typedef sensor_msgs::MagneticField    MagMsg;
+
+  typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
+  typedef message_filters::Subscriber<ImuMsg> ImuSubscriber;
+  typedef message_filters::Subscriber<MagMsg> MagSubscriber;
+
+  typedef imu_filter_madgwick::ImuFilterMadgwickConfig   FilterConfig;
+  typedef dynamic_reconfigure::Server<FilterConfig>   FilterConfigServer;
+
+  public:
+
+    ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private);
+    virtual ~ImuFilterRos();
+
+  private:
+
+    // **** ROS-related
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_private_;
+
+    boost::shared_ptr<ImuSubscriber> imu_subscriber_;
+    boost::shared_ptr<MagSubscriber> mag_subscriber_;
+    boost::shared_ptr<Synchronizer> sync_;
+
+    ros::Publisher rpy_filtered_debug_publisher_;
+    ros::Publisher rpy_raw_debug_publisher_;
+    ros::Publisher imu_publisher_;
+    tf2_ros::TransformBroadcaster tf_broadcaster_;
+
+    boost::shared_ptr<FilterConfigServer> config_server_;
+    ros::Timer check_topics_timer_;
+
+    // **** paramaters
+    WorldFrame::WorldFrame world_frame_;
+    bool use_mag_;
+    bool stateless_;
+    bool publish_tf_;
+    bool reverse_tf_;
+    std::string fixed_frame_;
+    std::string imu_frame_;
+    double constant_dt_;
+    bool publish_debug_topics_;
+    bool remove_gravity_vector_;
+    geometry_msgs::Vector3 mag_bias_;
+    double orientation_variance_;
+
+    // **** state variables
+    boost::mutex mutex_;
+    bool initialized_;
+    ros::Time last_time_;
+
+    // **** filter implementation
+    ImuFilter filter_;
+
+    // **** member functions
+    void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
+                        const MagMsg::ConstPtr& mav_msg);
+
+    void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw);
+
+    void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw);
+    void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw);
+
+    void publishRawMsg(const ros::Time& t,
+                       float roll, float pitch, float yaw);
+
+    void reconfigCallback(FilterConfig& config, uint32_t level);
+    void checkTopicsTimerCallback(const ros::TimerEvent&);
+};
+
+#endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h

@@ -0,0 +1,48 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H
+#define IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H
+
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Quaternion.h>
+#include <imu_filter_madgwick/world_frame.h>
+
+class StatelessOrientation
+{
+public:
+  static bool computeOrientation(
+    WorldFrame::WorldFrame frame,
+    geometry_msgs::Vector3 acceleration,
+    geometry_msgs::Vector3 magneticField,
+    geometry_msgs::Quaternion& orientation);
+
+  static bool computeOrientation(
+    WorldFrame::WorldFrame frame,
+    geometry_msgs::Vector3 acceleration,
+    geometry_msgs::Quaternion& orientation);
+
+};
+
+#endif // IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h

@@ -0,0 +1,8 @@
+#ifndef IMU_FILTER_MADGWICK_WORLD_FRAME_H_
+#define IMU_FILTER_MADGWICK_WORLD_FRAME_H_
+
+namespace WorldFrame {
+  enum WorldFrame { ENU, NED, NWU };
+}
+
+#endif

imu_tools/imu_filter_madgwick/launch/imu_filter_madgwick.launch

@@ -0,0 +1,17 @@
+<!-- Node for merging magnetometer and accelerometer data into a single imu message -->
+<launch>
+  #### Nodelet manager ######################################################
+
+  <node pkg="nodelet" type="nodelet" name="imu_manager" 
+    args="manager" output="screen" />
+
+  #### IMU Driver ###########################################################
+
+  <node pkg="nodelet" type="nodelet" name="ImuFilterNodelet" 
+    args="load imu_filter_madgwick/ImuFilterNodelet imu_manager" 
+    output="screen">
+    
+    <param name="publish_tf" value="false"/>
+
+  </node>
+</launch>

imu_tools/imu_filter_madgwick/package.xml

@@ -0,0 +1,44 @@
+<package>
+  <name>imu_filter_madgwick</name>
+  <version>1.2.3</version>
+  <description>  
+  Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into an orientation. Based on code by Sebastian Madgwick, http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms.
+  </description>
+  <license>GPL</license>
+  <url>http://ros.org/wiki/imu_filter_madgwick</url>
+  <author email="ivan.dryanovski@gmail.com">Ivan Dryanovski</author>
+  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+
+  <test_depend>rosunit</test_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/imu_filter_nodelet.xml" />
+  </export>
+
+</package>
+
+

imu_tools/imu_filter_madgwick/src/imu_filter.cpp

@@ -0,0 +1,338 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <cmath>
+#include "imu_filter_madgwick/imu_filter.h"
+
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root
+static float invSqrt(float x)
+{
+  float xhalf = 0.5f * x;
+  union
+  {
+    float x;
+    int i;
+  } u;
+  u.x = x;
+  u.i = 0x5f3759df - (u.i >> 1);
+  /* The next line can be repeated any number of times to increase accuracy */
+  u.x = u.x * (1.5f - xhalf * u.x * u.x);
+  return u.x;
+}
+
+template<typename T>
+static inline void normalizeVector(T& vx, T& vy, T& vz)
+{
+  T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz);
+  vx *= recipNorm;
+  vy *= recipNorm;
+  vz *= recipNorm;
+}
+
+template<typename T>
+static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3)
+{
+  T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+}
+
+static inline void rotateAndScaleVector(
+    float q0, float q1, float q2, float q3,
+    float _2dx, float _2dy, float _2dz,
+    float& rx, float& ry, float& rz) {
+
+  // result is half as long as input
+  rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
+     + _2dy * (q0 * q3 + q1 * q2)
+     + _2dz * (q1 * q3 - q0 * q2);
+  ry = _2dx * (q1 * q2 - q0 * q3)
+     + _2dy * (0.5f - q1 * q1 - q3 * q3)
+     + _2dz * (q0 * q1 + q2 * q3);
+  rz = _2dx * (q0 * q2 + q1 * q3)
+     + _2dy * (q2 * q3 - q0 * q1)
+     + _2dz * (0.5f - q1 * q1 - q2 * q2);
+}
+
+
+static inline void compensateGyroDrift(
+    float q0, float q1, float q2, float q3,
+    float s0, float s1, float s2, float s3,
+    float dt, float zeta,
+    float& w_bx, float& w_by, float& w_bz,
+    float& gx, float& gy, float& gz)
+{
+  // w_err = 2 q x s
+  float w_err_x = 2.0f * q0 * s1 - 2.0f * q1 * s0 - 2.0f * q2 * s3 + 2.0f * q3 * s2;
+  float w_err_y = 2.0f * q0 * s2 + 2.0f * q1 * s3 - 2.0f * q2 * s0 - 2.0f * q3 * s1;
+  float w_err_z = 2.0f * q0 * s3 - 2.0f * q1 * s2 + 2.0f * q2 * s1 - 2.0f * q3 * s0;
+
+  w_bx += w_err_x * dt * zeta;
+  w_by += w_err_y * dt * zeta;
+  w_bz += w_err_z * dt * zeta;
+
+  gx -= w_bx;
+  gy -= w_by;
+  gz -= w_bz;
+}
+
+static inline void orientationChangeFromGyro(
+    float q0, float q1, float q2, float q3,
+    float gx, float gy, float gz,
+    float& qDot1, float& qDot2, float& qDot3, float& qDot4)
+{
+  // Rate of change of quaternion from gyroscope
+  // See EQ 12
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+}
+
+static inline void addGradientDescentStep(
+    float q0, float q1, float q2, float q3,
+    float _2dx, float _2dy, float _2dz,
+    float mx, float my, float mz,
+    float& s0, float& s1, float& s2, float& s3)
+{
+  float f0, f1, f2;
+
+  // Gradient decent algorithm corrective step
+  // EQ 15, 21
+  rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2);
+
+  f0 -= mx;
+  f1 -= my;
+  f2 -= mz;
+
+
+  // EQ 22, 34
+  // Jt * f
+  s0 += (_2dy * q3 - _2dz * q2) * f0
+      + (-_2dx * q3 + _2dz * q1) * f1
+      + (_2dx * q2 - _2dy * q1) * f2;
+  s1 += (_2dy * q2 + _2dz * q3) * f0
+      + (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1
+      + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
+  s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
+      + (_2dx * q1 + _2dz * q3) * f1
+      + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
+  s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
+      + (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1
+      + (_2dx * q1 + _2dy * q2) * f2;
+}
+
+static inline void compensateMagneticDistortion(
+    float q0, float q1, float q2, float q3,
+    float mx, float my, float mz,
+    float& _2bxy, float& _2bz)
+{
+  float hx, hy, hz;
+  // Reference direction of Earth's magnetic field (See EQ 46)
+  rotateAndScaleVector(q0, -q1, -q2, -q3, mx, my, mz, hx, hy, hz);
+
+  _2bxy = 4.0f * sqrt (hx * hx + hy * hy);
+  _2bz = 4.0f * hz;
+
+}
+
+
+ImuFilter::ImuFilter() :
+    gain_ (0.0), zeta_ (0.0), world_frame_(WorldFrame::ENU),
+    q0(1.0), q1(0.0), q2(0.0), q3(0.0),
+    w_bx_(0.0), w_by_(0.0), w_bz_(0.0)
+{
+}
+
+ImuFilter::~ImuFilter()
+{
+}
+
+void ImuFilter::madgwickAHRSupdate(
+    float gx, float gy, float gz,
+    float ax, float ay, float az,
+    float mx, float my, float mz,
+    float dt)
+{
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float _2bz, _2bxy;
+
+  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+  if (!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz))
+  {
+    madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
+    return;
+  }
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
+  {
+    // Normalise accelerometer measurement
+    normalizeVector(ax, ay, az);
+
+    // Normalise magnetometer measurement
+    normalizeVector(mx, my, mz);
+
+    // Compensate for magnetic distortion
+    compensateMagneticDistortion(q0, q1, q2, q3, mx, my, mz, _2bxy, _2bz);
+
+    // Gradient decent algorithm corrective step
+    s0 = 0.0;  s1 = 0.0;  s2 = 0.0;  s3 = 0.0;
+    switch (world_frame_) {
+      case WorldFrame::NED:
+        // Gravity: [0, 0, -1]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
+
+        // Earth magnetic field: = [bxy, 0, bz]
+        addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
+        break;
+      case WorldFrame::NWU:
+        // Gravity: [0, 0, 1]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
+
+        // Earth magnetic field: = [bxy, 0, bz]
+        addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
+        break;
+      default:
+      case WorldFrame::ENU:
+        // Gravity: [0, 0, 1]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
+
+        // Earth magnetic field: = [0, bxy, bz]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, _2bxy, _2bz, mx, my, mz, s0, s1, s2, s3);
+        break;
+    }
+    normalizeQuaternion(s0, s1, s2, s3);
+
+    // compute gyro drift bias
+    compensateGyroDrift(q0, q1, q2, q3, s0, s1, s2, s3, dt, zeta_, w_bx_, w_by_, w_bz_, gx, gy, gz);
+
+    orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
+
+    // Apply feedback step
+    qDot1 -= gain_ * s0;
+    qDot2 -= gain_ * s1;
+    qDot3 -= gain_ * s2;
+    qDot4 -= gain_ * s3;
+  }
+  else
+  {
+    orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * dt;
+  q1 += qDot2 * dt;
+  q2 += qDot3 * dt;
+  q3 += qDot4 * dt;
+
+  // Normalise quaternion
+  normalizeQuaternion(q0, q1, q2, q3);
+}
+
+void ImuFilter::madgwickAHRSupdateIMU(
+    float gx, float gy, float gz,
+    float ax, float ay, float az,
+    float dt)
+{
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+
+  // Rate of change of quaternion from gyroscope
+  orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
+  {
+    // Normalise accelerometer measurement
+    normalizeVector(ax, ay, az);
+
+    // Gradient decent algorithm corrective step
+    s0 = 0.0;  s1 = 0.0;  s2 = 0.0;  s3 = 0.0;
+    switch (world_frame_) {
+      case WorldFrame::NED:
+        // Gravity: [0, 0, -1]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
+        break;
+      case WorldFrame::NWU:
+        // Gravity: [0, 0, 1]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
+        break;
+      default:
+      case WorldFrame::ENU:
+        // Gravity: [0, 0, 1]
+        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
+        break;
+    }
+
+    normalizeQuaternion(s0, s1, s2, s3);
+
+    // Apply feedback step
+    qDot1 -= gain_ * s0;
+    qDot2 -= gain_ * s1;
+    qDot3 -= gain_ * s2;
+    qDot4 -= gain_ * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * dt;
+  q1 += qDot2 * dt;
+  q2 += qDot3 * dt;
+  q3 += qDot4 * dt;
+
+  // Normalise quaternion
+  normalizeQuaternion (q0, q1, q2, q3);
+}
+
+
+void ImuFilter::getGravity(float& rx, float& ry, float& rz,
+    float gravity)
+{
+    // Estimate gravity vector from current orientation
+    switch (world_frame_) {
+      case WorldFrame::NED:
+        // Gravity: [0, 0, -1]
+        rotateAndScaleVector(q0, q1, q2, q3,
+            0.0, 0.0, -2.0*gravity,
+            rx, ry, rz);
+        break;
+      case WorldFrame::NWU:
+        // Gravity: [0, 0, 1]
+        rotateAndScaleVector(q0, q1, q2, q3,
+            0.0, 0.0, 2.0*gravity,
+            rx, ry, rz);
+        break;
+      default:
+      case WorldFrame::ENU:
+        // Gravity: [0, 0, 1]
+        rotateAndScaleVector(q0, q1, q2, q3,
+            0.0, 0.0, 2.0*gravity,
+            rx, ry, rz);
+        break;
+    }
+}

imu_tools/imu_filter_madgwick/src/imu_filter_node.cpp

@@ -0,0 +1,35 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "imu_filter_madgwick/imu_filter_ros.h"
+
+int main (int argc, char **argv)
+{
+  ros::init (argc, argv, "ImuFilter");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_private("~");
+  ImuFilterRos imu_filter(nh, nh_private);
+  ros::spin();
+  return 0;
+}

imu_tools/imu_filter_madgwick/src/imu_filter_nodelet.cpp

@@ -0,0 +1,39 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "imu_filter_madgwick/imu_filter_nodelet.h"
+#include <pluginlib/class_list_macros.h>
+
+void ImuFilterNodelet::onInit()
+{
+  NODELET_INFO("Initializing IMU Filter Nodelet");
+  
+  // TODO: Do we want the single threaded or multithreaded NH?
+  ros::NodeHandle nh         = getMTNodeHandle();
+  ros::NodeHandle nh_private = getMTPrivateNodeHandle();
+
+  filter_.reset(new ImuFilterRos(nh, nh_private));
+}
+
+PLUGINLIB_EXPORT_CLASS(ImuFilterNodelet, nodelet::Nodelet)

imu_tools/imu_filter_madgwick/src/imu_filter_ros.cpp

@@ -0,0 +1,393 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "imu_filter_madgwick/imu_filter_ros.h"
+#include "imu_filter_madgwick/stateless_orientation.h"
+#include "geometry_msgs/TransformStamped.h"
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+
+ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private):
+  nh_(nh),
+  nh_private_(nh_private),
+  initialized_(false)
+{
+  ROS_INFO ("Starting ImuFilter");
+
+  // **** get paramters
+  if (!nh_private_.getParam ("stateless", stateless_))
+    stateless_ = false;
+  if (!nh_private_.getParam ("use_mag", use_mag_))
+   use_mag_ = true;
+  if (!nh_private_.getParam ("publish_tf", publish_tf_))
+   publish_tf_ = true;
+  if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
+   reverse_tf_ = false;
+  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
+   fixed_frame_ = "odom";
+  if (!nh_private_.getParam ("constant_dt", constant_dt_))
+    constant_dt_ = 0.0;
+  if (!nh_private_.getParam ("remove_gravity_vector", remove_gravity_vector_))
+    remove_gravity_vector_= false;
+  if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
+    publish_debug_topics_= false;
+
+  std::string world_frame;
+  if (!nh_private_.getParam ("world_frame", world_frame))
+    world_frame = "enu";
+
+  if (world_frame == "ned") {
+    world_frame_ = WorldFrame::NED;
+  } else if (world_frame == "nwu"){
+    world_frame_ = WorldFrame::NWU;
+  } else if (world_frame == "enu"){
+    world_frame_ = WorldFrame::ENU;
+  } else {
+    ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
+    ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
+    world_frame_ = WorldFrame::ENU;
+  }
+  filter_.setWorldFrame(world_frame_);
+
+  // check for illegal constant_dt values
+  if (constant_dt_ < 0.0)
+  {
+    ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
+    constant_dt_ = 0.0;
+  }
+
+  // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
+  // otherwise, it will be constant
+  if (constant_dt_ == 0.0)
+    ROS_INFO("Using dt computed from message headers");
+  else
+    ROS_INFO("Using constant dt of %f sec", constant_dt_);
+
+  if (remove_gravity_vector_)
+    ROS_INFO("The gravity vector will be removed from the acceleration");
+  else
+    ROS_INFO("The gravity vector is kept in the IMU message.");
+
+  // **** register dynamic reconfigure
+  config_server_.reset(new FilterConfigServer(nh_private_));
+  FilterConfigServer::CallbackType f = boost::bind(&ImuFilterRos::reconfigCallback, this, _1, _2);
+  config_server_->setCallback(f);
+
+  // **** register publishers
+  imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
+    ros::names::resolve("imu") + "/data", 5);
+
+  if (publish_debug_topics_)
+  {
+    rpy_filtered_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
+      ros::names::resolve("imu") + "/rpy/filtered", 5);
+
+    rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
+      ros::names::resolve("imu") + "/rpy/raw", 5);
+  }
+
+  // **** register subscribers
+  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
+  int queue_size = 5;
+
+  imu_subscriber_.reset(new ImuSubscriber(
+    nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
+
+  if (use_mag_)
+  {
+    mag_subscriber_.reset(new MagSubscriber(
+      nh_, ros::names::resolve("imu") + "/mag", queue_size));
+
+    sync_.reset(new Synchronizer(
+      SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
+    sync_->registerCallback(boost::bind(&ImuFilterRos::imuMagCallback, this, _1, _2));
+  }
+  else
+  {
+    imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);
+  }
+
+  check_topics_timer_ = nh_.createTimer(ros::Duration(10.0), &ImuFilterRos::checkTopicsTimerCallback, this);
+}
+
+ImuFilterRos::~ImuFilterRos()
+{
+  ROS_INFO ("Destroying ImuFilter");
+
+  // Explicitly stop callbacks; they could execute after we're destroyed
+  check_topics_timer_.stop();
+}
+
+void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
+  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
+
+  ros::Time time = imu_msg_raw->header.stamp;
+  imu_frame_ = imu_msg_raw->header.frame_id;
+
+  if (!initialized_ || stateless_)
+  {
+    geometry_msgs::Quaternion init_q;
+    if (!StatelessOrientation::computeOrientation(world_frame_, lin_acc, init_q))
+    {
+      ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall, cannot determine gravity direction!");
+      return;
+    }
+    filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
+  }
+
+  if (!initialized_)
+  {
+    ROS_INFO("First IMU message received.");
+    check_topics_timer_.stop();
+
+    // initialize time
+    last_time_ = time;
+    initialized_ = true;
+  }
+
+  // determine dt: either constant, or from IMU timestamp
+  float dt;
+  if (constant_dt_ > 0.0)
+    dt = constant_dt_;
+  else
+  {
+    dt = (time - last_time_).toSec();
+    if (time.isZero())
+      ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
+                                    " The filter will not update the orientation.");
+  }
+
+  last_time_ = time;
+
+  if (!stateless_)
+    filter_.madgwickAHRSupdateIMU(
+      ang_vel.x, ang_vel.y, ang_vel.z,
+      lin_acc.x, lin_acc.y, lin_acc.z,
+      dt);
+
+  publishFilteredMsg(imu_msg_raw);
+  if (publish_tf_)
+    publishTransform(imu_msg_raw);
+}
+
+void ImuFilterRos::imuMagCallback(
+  const ImuMsg::ConstPtr& imu_msg_raw,
+  const MagMsg::ConstPtr& mag_msg)
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
+  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
+  const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
+
+  ros::Time time = imu_msg_raw->header.stamp;
+  imu_frame_ = imu_msg_raw->header.frame_id;
+
+  /*** Compensate for hard iron ***/
+  geometry_msgs::Vector3 mag_compensated;
+  mag_compensated.x = mag_fld.x - mag_bias_.x;
+  mag_compensated.y = mag_fld.y - mag_bias_.y;
+  mag_compensated.z = mag_fld.z - mag_bias_.z;
+
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
+  if (!initialized_ || stateless_)
+  {
+    // wait for mag message without NaN / inf
+    if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
+    {
+      return;
+    }
+
+    geometry_msgs::Quaternion init_q;
+    if (!StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q))
+    {
+      ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall or close to magnetic north pole, cannot determine gravity direction!");
+      return;
+    }
+    filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
+  }
+
+  if (!initialized_)
+  {
+    ROS_INFO("First pair of IMU and magnetometer messages received.");
+    check_topics_timer_.stop();
+
+    // initialize time
+    last_time_ = time;
+    initialized_ = true;
+  }
+
+  // determine dt: either constant, or from IMU timestamp
+  float dt;
+  if (constant_dt_ > 0.0)
+    dt = constant_dt_;
+  else
+  {
+    dt = (time - last_time_).toSec();
+    if (time.isZero())
+      ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
+                                    " The filter will not update the orientation.");
+  }
+
+  last_time_ = time;
+
+  if (!stateless_)
+    filter_.madgwickAHRSupdate(
+      ang_vel.x, ang_vel.y, ang_vel.z,
+      lin_acc.x, lin_acc.y, lin_acc.z,
+      mag_compensated.x, mag_compensated.y, mag_compensated.z,
+      dt);
+
+  publishFilteredMsg(imu_msg_raw);
+  if (publish_tf_)
+    publishTransform(imu_msg_raw);
+
+  if(publish_debug_topics_)
+  {
+    geometry_msgs::Quaternion orientation;
+    if (StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, orientation))
+    {
+      tf2::Matrix3x3(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)).getRPY(roll, pitch, yaw, 0);
+      publishRawMsg(time, roll, pitch, yaw);
+    }
+  }
+}
+
+void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
+{
+  double q0,q1,q2,q3;
+  filter_.getOrientation(q0,q1,q2,q3);
+  geometry_msgs::TransformStamped transform;
+  transform.header.stamp = imu_msg_raw->header.stamp;
+  if (reverse_tf_)
+  {
+    transform.header.frame_id = imu_frame_;
+    transform.child_frame_id = fixed_frame_;
+    transform.transform.rotation.w = q0;
+    transform.transform.rotation.x = -q1;
+    transform.transform.rotation.y = -q2;
+    transform.transform.rotation.z = -q3;
+  }
+  else {
+    transform.header.frame_id = fixed_frame_;
+    transform.child_frame_id = imu_frame_;
+    transform.transform.rotation.w = q0;
+    transform.transform.rotation.x = q1;
+    transform.transform.rotation.y = q2;
+    transform.transform.rotation.z = q3;
+  }
+  tf_broadcaster_.sendTransform(transform);
+
+}
+
+void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
+{
+  double q0,q1,q2,q3;
+  filter_.getOrientation(q0,q1,q2,q3);
+
+  // create and publish filtered IMU message
+  boost::shared_ptr<ImuMsg> imu_msg =
+    boost::make_shared<ImuMsg>(*imu_msg_raw);
+
+  imu_msg->orientation.w = q0;
+  imu_msg->orientation.x = q1;
+  imu_msg->orientation.y = q2;
+  imu_msg->orientation.z = q3;
+
+  imu_msg->orientation_covariance[0] = orientation_variance_;
+  imu_msg->orientation_covariance[1] = 0.0;
+  imu_msg->orientation_covariance[2] = 0.0;
+  imu_msg->orientation_covariance[3] = 0.0;
+  imu_msg->orientation_covariance[4] = orientation_variance_;
+  imu_msg->orientation_covariance[5] = 0.0;
+  imu_msg->orientation_covariance[6] = 0.0;
+  imu_msg->orientation_covariance[7] = 0.0;
+  imu_msg->orientation_covariance[8] = orientation_variance_;
+
+
+  if(remove_gravity_vector_) {
+    float gx, gy, gz;
+    filter_.getGravity(gx, gy, gz);
+    imu_msg->linear_acceleration.x -= gx;
+    imu_msg->linear_acceleration.y -= gy;
+    imu_msg->linear_acceleration.z -= gz;
+  }
+
+  imu_publisher_.publish(imu_msg);
+
+  if(publish_debug_topics_)
+  {
+    geometry_msgs::Vector3Stamped rpy;
+    tf2::Matrix3x3(tf2::Quaternion(q1,q2,q3,q0)).getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
+
+    rpy.header = imu_msg_raw->header;
+    rpy_filtered_debug_publisher_.publish(rpy);
+  }
+}
+
+void ImuFilterRos::publishRawMsg(const ros::Time& t,
+  float roll, float pitch, float yaw)
+{
+  geometry_msgs::Vector3Stamped rpy;
+  rpy.vector.x = roll;
+  rpy.vector.y = pitch;
+  rpy.vector.z = yaw ;
+  rpy.header.stamp = t;
+  rpy.header.frame_id = imu_frame_;
+  rpy_raw_debug_publisher_.publish(rpy);
+}
+
+
+void ImuFilterRos::reconfigCallback(FilterConfig& config, uint32_t level)
+{
+  double gain, zeta;
+  boost::mutex::scoped_lock lock(mutex_);
+  gain = config.gain;
+  zeta = config.zeta;
+  filter_.setAlgorithmGain(gain);
+  filter_.setDriftBiasGain(zeta);
+  ROS_INFO("Imu filter gain set to %f", gain);
+  ROS_INFO("Gyro drift bias set to %f", zeta);
+  mag_bias_.x = config.mag_bias_x;
+  mag_bias_.y = config.mag_bias_y;
+  mag_bias_.z = config.mag_bias_z;
+  orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
+  ROS_INFO("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
+}
+
+void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&)
+{
+  if (use_mag_)
+    ROS_WARN_STREAM("Still waiting for data on topics " << ros::names::resolve("imu") << "/data_raw"
+                    << " and " << ros::names::resolve("imu") << "/mag" << "...");
+  else
+    ROS_WARN_STREAM("Still waiting for data on topic " << ros::names::resolve("imu") << "/data_raw" << "...");
+}

imu_tools/imu_filter_madgwick/src/stateless_orientation.cpp

@@ -0,0 +1,172 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "imu_filter_madgwick/stateless_orientation.h"
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/convert.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+template<typename T>
+static inline void crossProduct(
+      T ax, T ay, T az,
+      T bx, T by, T bz,
+      T& rx, T& ry, T& rz) {
+  rx = ay*bz - az*by;
+  ry = az*bx - ax*bz;
+  rz = ax*by - ay*bx;
+}
+
+
+template<typename T>
+static inline T normalizeVector(T& vx, T& vy, T& vz) {
+  T norm = sqrt(vx*vx + vy*vy + vz*vz);
+  T inv = 1.0 / norm;
+  vx *= inv;
+  vy *= inv;
+  vz *= inv;
+  return norm;
+
+}
+
+
+bool StatelessOrientation::computeOrientation(
+  WorldFrame::WorldFrame frame,
+  geometry_msgs::Vector3 A,
+  geometry_msgs::Vector3 E,
+  geometry_msgs::Quaternion& orientation) {
+
+  float Hx, Hy, Hz;
+  float Mx, My, Mz;
+  float normH;
+
+  // A: pointing up
+  float Ax = A.x, Ay = A.y, Az = A.z;
+
+  // E: pointing down/north
+  float Ex = E.x, Ey = E.y, Ez = E.z;
+
+  // H: vector horizontal, pointing east
+  // H = E x A
+  crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
+
+  // normalize H
+  normH = normalizeVector(Hx, Hy, Hz);
+  if (normH < 1E-7) {
+    // device is close to free fall (or in space?), or close to
+    // magnetic north pole.
+    // mag in T => Threshold 1E-7, typical values are  > 1E-5.
+    return false;
+  }
+
+  // normalize A
+  normalizeVector(Ax, Ay, Az);
+
+  // M: vector horizontal, pointing north
+  // M = A x H
+  crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
+
+  // Create matrix for basis transformation
+  tf2::Matrix3x3 R;
+  switch (frame) {
+    case WorldFrame::NED:
+      // vector space world W:
+      // Basis: bwx (1,0,0) north, bwy (0,1,0) east, bwz (0,0,1) down
+      // vector space local L:
+      // Basis: M ,H, -A
+      // W(1,0,0) => L(M)
+      // W(0,1,0) => L(H)
+      // W(0,0,1) => L(-A)
+
+      // R: Transform Matrix local => world equals basis of L, because basis of W is I
+      R[0][0] = Mx;     R[0][1] = Hx;     R[0][2] = -Ax;
+      R[1][0] = My;     R[1][1] = Hy;     R[1][2] = -Ay;
+      R[2][0] = Mz;     R[2][1] = Hz;     R[2][2] = -Az;
+      break;
+
+    case WorldFrame::NWU:
+      // vector space world W:
+      // Basis: bwx (1,0,0) north, bwy (0,1,0) west, bwz (0,0,1) up
+      // vector space local L:
+      // Basis: M ,H, -A
+      // W(1,0,0) => L(M)
+      // W(0,1,0) => L(-H)
+      // W(0,0,1) => L(A)
+
+      // R: Transform Matrix local => world equals basis of L, because basis of W is I
+      R[0][0] = Mx;     R[0][1] = -Hx;     R[0][2] = Ax;
+      R[1][0] = My;     R[1][1] = -Hy;     R[1][2] = Ay;
+      R[2][0] = Mz;     R[2][1] = -Hz;     R[2][2] = Az;
+      break;
+
+    default:
+    case WorldFrame::ENU:
+      // vector space world W:
+      // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up
+      // vector space local L:
+      // Basis: H, M , A
+      // W(1,0,0) => L(H)
+      // W(0,1,0) => L(M)
+      // W(0,0,1) => L(A)
+
+      // R: Transform Matrix local => world equals basis of L, because basis of W is I
+      R[0][0] = Hx;     R[0][1] = Mx;     R[0][2] = Ax;
+      R[1][0] = Hy;     R[1][1] = My;     R[1][2] = Ay;
+      R[2][0] = Hz;     R[2][1] = Mz;     R[2][2] = Az;
+      break;
+  }
+
+  // Matrix.getRotation assumes vector rotation, but we're using
+  // coordinate systems. Thus negate rotation angle (inverse).
+  tf2::Quaternion q;
+  R.getRotation(q);
+  tf2::convert(q.inverse(), orientation);
+  return true;
+}
+
+
+bool StatelessOrientation::computeOrientation(
+  WorldFrame::WorldFrame frame,
+  geometry_msgs::Vector3 A,
+  geometry_msgs::Quaternion& orientation) {
+
+  // This implementation could be optimized regarding speed.
+
+  // magnetic Field E must not be parallel to A,
+  // choose an arbitrary orthogonal vector
+  geometry_msgs::Vector3 E;
+  if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) {
+      E.x = A.y;
+      E.y = A.x;
+      E.z = 0.0;
+  } else if (fabs(A.z) > 0.1) {
+      E.x = 0.0;
+      E.y = A.z;
+      E.z = A.y;
+  } else {
+      // free fall
+      return false;
+  }
+
+  return computeOrientation(frame, A, E, orientation);
+}

imu_tools/imu_filter_madgwick/test/madgwick_test.cpp

@@ -0,0 +1,121 @@
+#include <imu_filter_madgwick/imu_filter.h>
+#include <cmath>
+#include "test_helpers.h"
+
+#define FILTER_ITERATIONS 10000
+
+
+template <WorldFrame::WorldFrame FRAME>
+void filterStationary(
+    float Ax, float Ay, float Az,
+    float Mx, float My, float Mz,
+    double& q0, double& q1, double& q2, double& q3) {
+  float dt = 0.1;
+  float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0)
+
+  ImuFilter filter;
+  filter.setDriftBiasGain(0.0);
+  filter.setAlgorithmGain(0.1);
+
+  // initialize with some orientation
+  filter.setOrientation(q0,q1,q2,q3);
+  filter.setWorldFrame(FRAME);
+
+  for (int i = 0; i < FILTER_ITERATIONS; i++) {
+      filter.madgwickAHRSupdate(Gx, Gy, Gz, Ax, Ay, Az, Mx, My, Mz, dt);
+  }
+
+  filter.getOrientation(q0,q1,q2,q3);
+}
+
+
+template <WorldFrame::WorldFrame FRAME>
+void filterStationary(
+    float Ax, float Ay, float Az,
+    double& q0, double& q1, double& q2, double& q3) {
+  float dt = 0.1;
+  float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0)
+
+  ImuFilter filter;
+  filter.setDriftBiasGain(0.0);
+  filter.setAlgorithmGain(0.1);
+
+  // initialize with some orientation
+  filter.setOrientation(q0,q1,q2,q3);
+  filter.setWorldFrame(FRAME);
+
+  for (int i = 0; i < FILTER_ITERATIONS; i++) {
+      filter.madgwickAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az, dt);
+  }
+
+  filter.getOrientation(q0,q1,q2,q3);
+}
+
+
+#define TEST_STATIONARY_ENU(in_am, exp_result)       \
+  TEST(MadgwickTest, Stationary_ENU_ ## in_am){      \
+  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
+  filterStationary<WorldFrame::ENU>(in_am, q0, q1, q2, q3);  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
+  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }   \
+  TEST(MadgwickTest, Stationary_ENU_NM_ ## in_am){   \
+  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
+  filterStationary<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3);  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
+  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
+
+#define TEST_STATIONARY_NED(in_am, exp_result)       \
+  TEST(MadgwickTest, Stationary_NED_ ## in_am){      \
+  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
+  filterStationary<WorldFrame::NED>(in_am, q0, q1, q2, q3);  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
+  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }   \
+  TEST(MadgwickTest, Stationary_NED_NM_ ## in_am){   \
+  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
+  filterStationary<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3);  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
+  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
+
+#define TEST_STATIONARY_NWU(in_am, exp_result)       \
+  TEST(MadgwickTest, Stationary_NWU_ ## in_am){      \
+  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
+  filterStationary<WorldFrame::NWU>(in_am, q0, q1, q2, q3);  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
+  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }   \
+  TEST(MadgwickTest, Stationary_NWU_NM_ ## in_am){   \
+  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
+  filterStationary<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3);  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
+  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
+
+TEST_STATIONARY_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180)
+TEST_STATIONARY_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY)
+TEST_STATIONARY_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU)
+TEST_STATIONARY_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU)
+
+TEST_STATIONARY_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY)
+TEST_STATIONARY_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120)
+TEST_STATIONARY_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90)
+TEST_STATIONARY_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU)
+TEST_STATIONARY_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU)
+
+TEST_STATIONARY_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY)
+TEST_STATIONARY_NED(AM_NORTH_WEST_UP, QUAT_X_180)
+TEST_STATIONARY_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
+TEST_STATIONARY_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED)
+
+
+
+TEST(MadgwickTest, TestQuatEqNoZ) {
+
+  ASSERT_TRUE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_MZ_90));
+  ASSERT_FALSE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_X_180));
+
+}
+
+
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

imu_tools/imu_filter_madgwick/test/stateless_orientation_test.cpp

@@ -0,0 +1,117 @@
+#include <imu_filter_madgwick/stateless_orientation.h>
+#include "test_helpers.h"
+
+
+template<WorldFrame::WorldFrame FRAME>
+bool computeOrientation(
+    float Ax, float Ay, float Az,
+    float Mx, float My, float Mz,
+    double& q0, double& q1, double& q2, double& q3) {
+
+  geometry_msgs::Vector3 A, M;
+  geometry_msgs::Quaternion orientation;
+  A.x = Ax; A.y = Ay; A.z = Az;
+  M.x = Mx; M.y = My; M.z = Mz;
+
+  bool res = StatelessOrientation::computeOrientation(FRAME, A, M, orientation);
+
+  q0 = orientation.w;
+  q1 = orientation.x;
+  q2 = orientation.y;
+  q3 = orientation.z;
+
+  return res;
+}
+
+template<WorldFrame::WorldFrame FRAME>
+bool computeOrientation(
+    float Ax, float Ay, float Az,
+    double& q0, double& q1, double& q2, double& q3) {
+
+  geometry_msgs::Vector3 A;
+  geometry_msgs::Quaternion orientation;
+  A.x = Ax; A.y = Ay; A.z = Az;
+
+  bool res = StatelessOrientation::computeOrientation(FRAME, A, orientation);
+
+  q0 = orientation.w;
+  q1 = orientation.x;
+  q2 = orientation.y;
+  q3 = orientation.z;
+
+  return res;
+}
+
+
+#define TEST_STATELESS_ENU(in_am, exp_result)       \
+  TEST(StatelessOrientationTest, Stationary_ENU_ ## in_am){      \
+  double q0, q1, q2, q3;                                         \
+  ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(in_am, q0, q1, q2, q3)); \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
+  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }               \
+  TEST(StatelessOrientationTest, Stationary_ENU_NM_ ## in_am){   \
+  double q0, q1, q2, q3;                                         \
+  ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
+  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
+
+#define TEST_STATELESS_NED(in_am, exp_result)       \
+  TEST(StatelessOrientationTest, Stationary_NED_ ## in_am){      \
+  double q0, q1, q2, q3;                                         \
+  ASSERT_TRUE(computeOrientation<WorldFrame::NED>(in_am, q0, q1, q2, q3));  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
+  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }               \
+  TEST(StatelessOrientationTest, Stationary_NED_NM_ ## in_am){   \
+  double q0, q1, q2, q3;                                         \
+  ASSERT_TRUE(computeOrientation<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
+  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
+
+#define TEST_STATELESS_NWU(in_am, exp_result)       \
+  TEST(StatelessOrientationTest, Stationary_NWU_ ## in_am){      \
+  double q0, q1, q2, q3;                                         \
+  ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(in_am, q0, q1, q2, q3));  \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
+  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }               \
+  TEST(StatelessOrientationTest, Stationary_NWU_NM_ ## in_am){   \
+  double q0, q1, q2, q3;                                         \
+  ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
+  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
+  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
+
+TEST_STATELESS_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY)
+TEST_STATELESS_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120)
+TEST_STATELESS_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90)
+TEST_STATELESS_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU)
+TEST_STATELESS_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU)
+
+
+TEST_STATELESS_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY)
+TEST_STATELESS_NED(AM_WEST_NORTH_DOWN, QUAT_MZ_90)
+TEST_STATELESS_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
+TEST_STATELESS_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED)
+
+
+TEST_STATELESS_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180)
+TEST_STATELESS_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY)
+TEST_STATELESS_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU)
+TEST_STATELESS_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU)
+
+
+TEST(StatelessOrientationTest, Check_NoAccel){
+  double q0, q1, q2, q3;
+  ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
+    0.0, 0.0, 0.0,
+    0.0, 0.0005, -0.0005,
+    q0, q1, q2, q3));
+}
+
+TEST(StatelessOrientationTest, Check_NoMag){
+  double q0, q1, q2, q3;
+  ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
+    0.0, 0.0, 9.81,
+    0.0, 0.0, 0.0,
+    q0, q1, q2, q3));
+}
+
+

imu_tools/imu_filter_madgwick/test/test_helpers.h

@@ -0,0 +1,102 @@
+
+#ifndef TEST_TEST_HELPERS_H_
+#define TEST_TEST_HELPERS_H_
+
+#include <gtest/gtest.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#define MAX_DIFF 0.05
+
+template <typename T>
+static inline void normalize_quaternion(T& q0, T& q1, T& q2, T& q3) {
+  T invNorm = 1 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+  T max = q0;
+  if (fabs(max) < fabs(q1)) max = q1;
+  if (fabs(max) < fabs(q2)) max = q2;
+  if (fabs(max) < fabs(q3)) max = q3;
+  if (max < 0) invNorm *= -1.0;
+
+  q0 *= invNorm;
+  q1 *= invNorm;
+  q2 *= invNorm;
+  q3 *= invNorm;
+}
+
+// Tests for normalization in the same way as tf2:
+// https://github.com/ros/geometry2/blob/bd490515b1434caeff521ea14901dfe04063ca27/tf2/src/buffer_core.cpp#L244-L247
+template <typename T>
+static inline bool is_normalized(T q0, T q1, T q2, T q3) {
+  return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0f) < 10e-6;
+}
+
+template <typename T>
+static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
+  normalize_quaternion(q0, q1, q2, q3);
+  normalize_quaternion(qr0, qr1, qr2, qr3);
+
+  return (fabs(q0 - qr0) < MAX_DIFF) &&
+         (fabs(q1 - qr1) < MAX_DIFF) &&
+         (fabs(q2 - qr2) < MAX_DIFF) &&
+         (fabs(q3 - qr3) < MAX_DIFF);
+}
+
+template <typename T>
+static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
+  // assert q == qr * qz
+  tf2::Quaternion q(q1, q2, q3, q0);
+  tf2::Quaternion qr(qr1, qr2, qr3, qr0);
+  tf2::Quaternion qz = q * qr.inverse();
+
+  // remove x and y components.
+  qz.setX(0.0);
+  qz.setY(0.0);
+
+  tf2::Quaternion qr_ = qz * qr;
+
+  return quat_equal(q0, q1, q2, q3,
+      qr_.getW(),
+      qr_.getX(),
+      qr_.getY(),
+      qr_.getZ());
+}
+
+#define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
+#define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__)
+
+#define ASSERT_QUAT_EQUAL_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_equal(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
+#define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__)
+
+#define ASSERT_QUAT_EQUAL_EX_Z_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_eq_ex_z(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
+#define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__)
+
+// Well known states
+// scheme: AM_x_y_z
+#define AM_EAST_NORTH_UP    /* Acceleration */ 0.0, 0.0, 9.81,  /* Magnetic */ 0.0, 0.0005, -0.0005
+#define AM_NORTH_EAST_DOWN  /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0005, 0.0, 0.0005
+#define AM_NORTH_WEST_UP    /* Acceleration */ 0.0, 0.0, 9.81,  /* Magnetic */ 0.0005, 0.0, -0.0005
+#define AM_SOUTH_UP_WEST    /* Acceleration */ 0.0, 9.81, 0.0,  /* Magnetic */ -0.0005, -0.0005, 0.0
+#define AM_SOUTH_EAST_UP    /* Acceleration */ 0.0, 0.0, 9.81,  /* Magnetic */ -0.0005, 0.0, -0.0005
+#define AM_WEST_NORTH_DOWN  /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0, 0.0005, 0.0005
+
+// Real sensor data
+#define AM_WEST_NORTH_DOWN_RSD /* Acceleration */ 0.12, 0.29, -9.83, /* Magnetic */ 6.363e-06, 1.0908e-05, 4.2723e-05
+#define AM_NE_NW_UP_RSD   /* Acceleration */ 0.20, 0.55, 9.779, /* Magnetic */ 8.484e-06, 8.181e-06, -4.3329e-05
+
+// Strip accelration from am
+#define ACCEL_ONLY(ax,ay,az, mx,my,mz) ax, ay, az
+
+// Well known quaternion
+// scheme: QUAT_axis_angle
+#define QUAT_IDENTITY  1.0, 0.0, 0.0, 0.0
+#define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107
+#define QUAT_X_180 0.0, 1.0, 0.0, 0.0
+#define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5
+#define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012 /* axis: (0.864401, 0.502559, 0.0120614) | angle: 178.848deg */
+#define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02/* Axis: (-0.2, -0.96, 0.02), Angle: 180deg */
+#define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50 /* Axis: -Z, Angle: 60deg */
+#define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41 /* axis: (0.0300376, -0.020025, -0.410513) | angle: 48.6734deg */
+#define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35 /* Axis: Z,  Angle: 41deg */
+#define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02 /* Axis: (0.9, 0.4, 0.0), Angle: 180deg */
+
+
+#endif /* TEST_TEST_HELPERS_H_ */

imu_tools/imu_tools/CHANGELOG.rst

@@ -0,0 +1,81 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_tools
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.2.3 (2021-04-09)
+------------------
+* Fix "non standard content" warning in imu_tools metapackage
+  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
+* Update maintainers in package.xml
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.2.2 (2020-05-25)
+------------------
+
+1.2.1 (2019-05-06)
+------------------
+
+1.2.0 (2018-05-25)
+------------------
+
+1.1.5 (2017-05-24)
+------------------
+
+1.1.4 (2017-05-22)
+------------------
+
+1.1.3 (2017-03-10)
+------------------
+
+1.1.2 (2016-09-07)
+------------------
+
+1.1.1 (2016-09-07)
+------------------
+
+1.1.0 (2016-04-25)
+------------------
+
+1.0.11 (2016-04-22)
+-------------------
+
+1.0.10 (2016-04-22)
+-------------------
+
+1.0.9 (2015-10-16)
+------------------
+
+1.0.8 (2015-10-07)
+------------------
+* Add imu_complementary_filter to meta package
+* Contributors: Martin Günther
+
+1.0.7 (2015-10-07)
+------------------
+
+1.0.6 (2015-10-06)
+------------------
+
+1.0.5 (2015-06-24)
+------------------
+
+1.0.4 (2015-05-06)
+------------------
+
+1.0.3 (2015-01-29)
+------------------
+
+1.0.2 (2015-01-27)
+------------------
+
+1.0.1 (2014-12-10)
+------------------
+* add me as maintainer to package.xml
+* Contributors: Martin Günther
+
+1.0.0 (2014-09-03)
+------------------
+* First public release
+* catkinization of imu_tools metapackage
+* Contributors: Francisco Vina

imu_tools/imu_tools/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(imu_tools)
+find_package(catkin REQUIRED)
+catkin_metapackage()

imu_tools/imu_tools/package.xml

@@ -0,0 +1,21 @@
+<package>
+  <name> imu_tools </name>
+  <version>1.2.3</version>
+  <description>
+    Various tools for IMU devices
+  </description>
+  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
+  <license>BSD, GPL</license>
+
+  <url>http://ros.org/wiki/imu_tools</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>imu_complementary_filter</run_depend>
+  <run_depend>imu_filter_madgwick</run_depend>
+  <run_depend>rviz_imu_plugin</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>

imu_tools/rviz_imu_plugin/CHANGELOG.rst

@@ -0,0 +1,100 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rviz_imu_plugin
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.2.3 (2021-04-09)
+------------------
+* Fix "non standard content" warning in imu_tools metapackage
+  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
+* Update maintainers in package.xml
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.2.2 (2020-05-25)
+------------------
+* Export symbols so plugin can load
+* properly show/hide visualization when enabled/disabled
+* Contributors: CCNY Robotics Lab, Lou Amadio, Martin Günther, v4hn
+
+1.2.1 (2019-05-06)
+------------------
+* Fix includes, typos and log messages
+* print ros_warn and give unit quaternion to ogre to prevent rviz crash (`#90 <https://github.com/ccny-ros-pkg/imu_tools/issues/90>`_)
+* Contributors: Jackey-Huo, Martin Günther
+
+1.2.0 (2018-05-25)
+------------------
+
+1.1.5 (2017-05-24)
+------------------
+
+1.1.4 (2017-05-22)
+------------------
+* Add option to display orientation in world frame (`#69 <https://github.com/ccny-ros-pkg/imu_tools/issues/69>`_)
+  Per REP 145 IMU orientation is in the world frame. Rotating the
+  orientation data to transform into the sensor frame results in strange
+  behavior, such as double-rotation of orientation on a robot. Provide an
+  option to display orientation in the world frame, and enable it by
+  default. Continue to translate the position of the data to the sensor
+  frame.
+* Contributors: C. Andy Martin
+
+1.1.3 (2017-03-10)
+------------------
+
+1.1.2 (2016-09-07)
+------------------
+
+1.1.1 (2016-09-07)
+------------------
+
+1.1.0 (2016-04-25)
+------------------
+* Add qt5 dependencies to rviz_imu_plugin package.xml
+  This fixes the compilation errors on Kinetic for Debian Jessie.
+* Contributors: Martin Guenther
+
+1.0.11 (2016-04-22)
+-------------------
+
+1.0.10 (2016-04-22)
+-------------------
+* Support qt4/qt5 using rviz's exported qt version
+  Closes `#58 <https://github.com/ccny-ros-pkg/imu_tools/issues/58>`_ .
+  This fixes the build on Kinetic, where only Qt5 is available, and
+  is backwards compatible with Qt4 for Indigo.
+* Contributors: Martin Guenther
+
+1.0.9 (2015-10-16)
+------------------
+
+1.0.8 (2015-10-07)
+------------------
+
+1.0.7 (2015-10-07)
+------------------
+
+1.0.6 (2015-10-06)
+------------------
+
+1.0.5 (2015-06-24)
+------------------
+
+1.0.4 (2015-05-06)
+------------------
+
+1.0.3 (2015-01-29)
+------------------
+
+1.0.2 (2015-01-27)
+------------------
+
+1.0.1 (2014-12-10)
+------------------
+* add me as maintainer to package.xml
+* Contributors: Martin Günther
+
+1.0.0 (2014-09-03)
+------------------
+* First public release
+* Contributors: Ivan Dryanovski, Martin Günther, Davide Tateo, Francisco Vina, Lorenzo Riano

imu_tools/rviz_imu_plugin/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(rviz_imu_plugin)
+
+find_package(catkin REQUIRED COMPONENTS rviz)
+
+## This setting causes Qt's "MOC" generation to happen automatically.
+set(CMAKE_AUTOMOC ON)
+
+## This plugin includes Qt widgets, so we must include Qt.
+## We'll use the version that rviz used so they are compatible.
+if(rviz_QT_VERSION VERSION_LESS "5")
+  message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
+  include(${QT_USE_FILE})
+else()
+  message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt5Widgets REQUIRED)
+  find_package(Qt5Core REQUIRED)
+  find_package(Qt5OpenGL REQUIRED)
+  ## Set the QT_LIBRARIES variable for Qt5, so it can be used below.
+  set(QT_LIBRARIES Qt5::Widgets)
+endif()
+
+## I prefer the Qt signals and slots to avoid defining "emit", "slots",
+## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here.
+add_definitions(-DQT_NO_KEYWORDS)
+
+catkin_package(
+  DEPENDS 
+  CATKIN_DEPENDS rviz
+  INCLUDE_DIRS
+  LIBRARIES
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Here we specify the list of source files.
+## The generated MOC files are included automatically as headers.
+set(SRC_FILES  src/imu_display.cpp
+               src/imu_orientation_visual.cpp
+               src/imu_axes_visual.cpp
+               src/imu_acc_visual.cpp)
+
+## Build libraries
+add_library(${PROJECT_NAME} ${SRC_FILES})
+
+set_target_properties(${PROJECT_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
+
+## Link the library with whatever Qt libraries have been defined by
+## the ``find_package(Qt4 ...)`` line above, or by the
+## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries
+## catkin has included.
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

imu_tools/rviz_imu_plugin/package.xml

@@ -0,0 +1,28 @@
+<package>
+  <name>rviz_imu_plugin</name>
+  <version>1.2.3</version>
+  <description>
+    RVIZ plugin for IMU visualization
+  </description>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/rviz_imu_plugin</url>
+  <author email="ivan.dryanovski@gmail.com">Ivan Dryanovski</author>
+  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>qtbase5-dev</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rviz</build_depend>
+
+  <run_depend>libqt5-core</run_depend>
+  <run_depend>libqt5-gui</run_depend>
+  <run_depend>libqt5-widgets</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rviz</run_depend>
+
+  <export>
+    <rosdoc config="${prefix}/rosdoc.yaml"/>
+    <rviz plugin="${prefix}/plugin_description.xml"/>
+  </export>
+</package>

imu_tools/rviz_imu_plugin/plugin_description.xml

@@ -0,0 +1,13 @@
+<library path="lib/librviz_imu_plugin">
+
+  <class name="rviz_imu_plugin/Imu"
+         type="rviz::ImuDisplay"
+         base_class_type="rviz::Display">
+
+    <description>
+      Displays the orientation and acceleration components of sensor_msgs/Imu messages.
+    </description>
+
+  </class>
+
+</library>

imu_tools/rviz_imu_plugin/rosdoc.yaml

@@ -0,0 +1,2 @@
+- builder: sphinx
+  sphinx_root_dir: src/doc

imu_tools/rviz_imu_plugin/src/imu_acc_visual.cpp

@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "imu_acc_visual.h"
+
+namespace rviz
+{
+
+ImuAccVisual::ImuAccVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node):
+  acc_vector_(NULL),
+  arrow_length_(9.81),
+  arrow_radius_(0.50),
+  head_length_(1.00),
+  head_radius_(1.00),
+  scale_(0.05),
+  alpha_(1.0),
+  color_(1.0, 1.0, 0.0),
+  derotated_(true)
+{
+  scene_manager_ = scene_manager;
+
+  // Ogre::SceneNode s form a tree, with each node storing the
+  // transform (position and orientation) of itself relative to its
+  // parent.  Ogre does the math of combining those transforms when it
+  // is time to render.
+  //
+  // Here we create a node to store the pose of the Imu's header frame
+  // relative to the RViz fixed frame.
+  frame_node_ = parent_node->createChildSceneNode();
+}
+
+ImuAccVisual::~ImuAccVisual()
+{
+  hide();
+
+  // Destroy the frame node since we don't need it anymore.
+  scene_manager_->destroySceneNode(frame_node_);
+}
+
+void ImuAccVisual::show()
+{
+  if (!acc_vector_)
+  {
+    acc_vector_ = new Arrow(scene_manager_, frame_node_);
+    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
+    acc_vector_->setDirection(direction_);
+    acc_vector_->set(
+      arrow_length_ * scale_, 
+      arrow_radius_ * scale_, 
+      head_length_  * scale_, 
+      head_radius_  * scale_);
+  }
+}
+
+void ImuAccVisual::hide()
+{
+  if (acc_vector_)
+  {
+    delete acc_vector_;
+    acc_vector_ = NULL;
+  }
+}
+
+void ImuAccVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg)
+{
+  direction_ = Ogre::Vector3(msg->linear_acceleration.x,
+                             msg->linear_acceleration.y,
+                             msg->linear_acceleration.z);
+
+
+  // Rotate the acceleration vector by the IMU orientation. This makes
+  // sense since the visualization of the IMU is also rotated by the 
+  // orientation. In this way, both appear in the inertial frame.
+  if (derotated_)
+  {
+    Ogre::Quaternion orientation(msg->orientation.w,
+                                 msg->orientation.x,
+                                 msg->orientation.y,
+                                 msg->orientation.z);
+
+    direction_ = orientation * direction_;
+  }
+
+  arrow_length_ = sqrt(
+    msg->linear_acceleration.x * msg->linear_acceleration.x +
+    msg->linear_acceleration.y * msg->linear_acceleration.y +
+    msg->linear_acceleration.z * msg->linear_acceleration.z);
+
+  if (acc_vector_)
+  {
+    acc_vector_->setDirection(direction_);
+    acc_vector_->set(
+      arrow_length_ * scale_, 
+      arrow_radius_ * scale_, 
+      head_length_  * scale_, 
+      head_radius_  * scale_);
+  }
+}
+
+void ImuAccVisual::setScale(float scale) 
+{ 
+  scale_ = scale; 
+  if (acc_vector_)
+  {
+    acc_vector_->setDirection(direction_);
+    acc_vector_->set(
+      arrow_length_ * scale_, 
+      arrow_radius_ * scale_, 
+      head_length_  * scale_, 
+      head_radius_  * scale_);
+  }
+}
+
+void ImuAccVisual::setColor(const QColor& color)
+{
+  color_ = color;
+  if (acc_vector_) 
+    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
+}
+
+void ImuAccVisual::setAlpha(float alpha) 
+{ 
+  alpha_ = alpha; 
+  if (acc_vector_) 
+    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(),alpha_);
+}
+
+void ImuAccVisual::setDerotated(bool derotated) 
+{ 
+  derotated_ = derotated; 
+  if (acc_vector_) 
+    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(),alpha_);
+}
+
+void ImuAccVisual::setFramePosition(const Ogre::Vector3& position)
+{
+  frame_node_->setPosition(position);
+}
+
+void ImuAccVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
+{
+  frame_node_->setOrientation(orientation);
+}
+
+} // end namespace rviz
+

imu_tools/rviz_imu_plugin/src/imu_acc_visual.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H
+#define RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H
+
+#include <sensor_msgs/Imu.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include <rviz/ogre_helpers/arrow.h>
+#include <QColor>
+
+
+namespace rviz
+{
+
+class ImuAccVisual
+{
+  public:
+    // Constructor.  Creates the visual stuff and puts it into the
+    // scene, but in an unconfigured state.
+    ImuAccVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
+
+    // Destructor.  Removes the visual stuff from the scene.
+    virtual ~ImuAccVisual();
+
+    // Configure the visual to show the data in the message.
+    void setMessage(const sensor_msgs::Imu::ConstPtr& msg);
+
+    // Set the pose of the coordinate frame the message refers to.
+    // These could be done inside setMessage(), but that would require
+    // calls to FrameManager and error handling inside setMessage(),
+    // which doesn't seem as clean.  This way ImuVisual is only
+    // responsible for visualization.
+    void setFramePosition(const Ogre::Vector3& position);
+    void setFrameOrientation(const Ogre::Quaternion& orientation);
+
+    // Set the color and alpha of the visual, which are user-editable
+
+    void setScale(float scale);
+    void setColor(const QColor &color);
+    void setAlpha(float alpha);
+    void setDerotated(bool derotated);
+
+    float getScale() { return scale_; }
+    const QColor& getColor() { return color_; }
+    float getAlpha() { return alpha_; }
+    bool  getDerotated() { return derotated_; }
+
+    void show();
+    void hide();
+
+  private:
+
+    void create();
+
+    Arrow * acc_vector_;
+
+    Ogre::Vector3 direction_; // computed from IMU message
+
+    float arrow_length_; // computed from IMU message
+    float arrow_radius_;
+    float head_length_;
+    float head_radius_;
+
+    float scale_;
+    float alpha_;
+    QColor color_;
+
+    bool derotated_;
+ 
+    // A SceneNode whose pose is set to match the coordinate frame of
+    // the Imu message header.
+    Ogre::SceneNode * frame_node_;
+
+    // The SceneManager, kept here only so the destructor can ask it to
+    // destroy the ``frame_node_``.
+    Ogre::SceneManager * scene_manager_;
+};
+
+} // end namespace rviz
+
+#endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H

imu_tools/rviz_imu_plugin/src/imu_axes_visual.cpp

@@ -0,0 +1,144 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "imu_axes_visual.h"
+
+#include <ros/ros.h>
+#include <cmath>
+
+namespace rviz
+{
+
+ImuAxesVisual::ImuAxesVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node):
+  orientation_axes_(NULL),
+  scale_(0.15), quat_valid_(true)
+{
+  scene_manager_ = scene_manager;
+
+  // Ogre::SceneNode s form a tree, with each node storing the
+  // transform (position and orientation) of itself relative to its
+  // parent.  Ogre does the math of combining those transforms when it
+  // is time to render.
+  //
+  // Here we create a node to store the pose of the Imu's header frame
+  // relative to the RViz fixed frame.
+  frame_node_ = parent_node->createChildSceneNode();
+}
+
+ImuAxesVisual::~ImuAxesVisual()
+{
+  hide();
+
+  // Destroy the frame node since we don't need it anymore.
+  scene_manager_->destroySceneNode(frame_node_);
+}
+
+void ImuAxesVisual::show()
+{
+  if (!orientation_axes_)
+  {
+    orientation_axes_ = new Axes(scene_manager_, frame_node_);
+    orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_));
+    orientation_axes_->setOrientation(orientation_);
+  }
+}
+
+void ImuAxesVisual::hide()
+{
+  if (orientation_axes_)
+  {
+    delete orientation_axes_;
+    orientation_axes_ = NULL;
+  }
+}
+
+void ImuAxesVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg)
+{
+  if (checkQuaternionValidity(msg)) {
+    if (!quat_valid_) {
+      ROS_INFO("rviz_imu_plugin got valid quaternion, "
+               "displaying true orientation");
+      quat_valid_ = true;
+    }
+    orientation_ = Ogre::Quaternion(msg->orientation.w,
+                                    msg->orientation.x,
+                                    msg->orientation.y,
+                                    msg->orientation.z);
+  } else {
+    if (quat_valid_) {
+      ROS_WARN("rviz_imu_plugin got invalid quaternion (%lf, %lf, %lf, %lf), "
+               "will display neutral orientation instead", msg->orientation.w,
+               msg->orientation.x,msg->orientation.y,msg->orientation.z);
+      quat_valid_ = false;
+    }
+    // if quaternion is invalid, give a unit quat to Ogre
+    orientation_ = Ogre::Quaternion();
+  }
+
+  if (orientation_axes_)
+    orientation_axes_->setOrientation(orientation_);
+}
+
+void ImuAxesVisual::setScale(float scale) 
+{ 
+  scale_ = scale; 
+  if (orientation_axes_) 
+   orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_));
+}
+
+void ImuAxesVisual::setFramePosition(const Ogre::Vector3& position)
+{
+  frame_node_->setPosition(position);
+}
+
+void ImuAxesVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
+{
+  frame_node_->setOrientation(orientation);
+}
+
+inline bool ImuAxesVisual::checkQuaternionValidity(
+    const sensor_msgs::Imu::ConstPtr& msg) {
+
+  double x = msg->orientation.x,
+         y = msg->orientation.y,
+         z = msg->orientation.z,
+         w = msg->orientation.w;
+  // OGRE can handle unnormalized quaternions, but quat's length extremely small;
+  // this may indicate that invalid (0, 0, 0, 0) quat is passed, this will lead ogre
+  // to crash unexpectly
+  if ( std::sqrt( x*x + y*y + z*z + w*w ) < 0.0001 ) {
+    return false;
+  }
+  return true;
+}
+
+
+} // end namespace rviz
+

imu_tools/rviz_imu_plugin/src/imu_axes_visual.h

@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H
+#define RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H
+
+#include <sensor_msgs/Imu.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include <rviz/ogre_helpers/axes.h>
+
+namespace rviz
+{
+
+class Axes;
+
+class ImuAxesVisual
+{
+  public:
+    // Constructor.  Creates the visual stuff and puts it into the
+    // scene, but in an unconfigured state.
+    ImuAxesVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
+
+    // Destructor.  Removes the visual stuff from the scene.
+    virtual ~ImuAxesVisual();
+
+    // Configure the visual to show the data in the message.
+    void setMessage(const sensor_msgs::Imu::ConstPtr& msg);
+
+    // Set the pose of the coordinate frame the message refers to.
+    // These could be done inside setMessage(), but that would require
+    // calls to FrameManager and error handling inside setMessage(),
+    // which doesn't seem as clean.  This way ImuVisual is only
+    // responsible for visualization.
+    void setFramePosition(const Ogre::Vector3& position);
+    void setFrameOrientation(const Ogre::Quaternion& orientation);
+
+    // Set the color and alpha of the visual, which are user-editable
+
+    void setScale(float scale);
+
+    float getScale() { return scale_; }
+
+    void show();
+    void hide();
+
+  private:
+
+    void create();
+    inline bool checkQuaternionValidity(
+        const sensor_msgs::Imu::ConstPtr& msg);
+
+    Ogre::Quaternion orientation_;
+
+    float scale_;
+    bool quat_valid_;
+
+    Axes * orientation_axes_;
+  
+    // A SceneNode whose pose is set to match the coordinate frame of
+    // the Imu message header.
+    Ogre::SceneNode * frame_node_;
+
+    // The SceneManager, kept here only so the destructor can ask it to
+    // destroy the ``frame_node_``.
+    Ogre::SceneManager * scene_manager_;
+};
+
+} // end namespace rviz
+
+#endif //  RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H

imu_tools/rviz_imu_plugin/src/imu_display.cpp

@@ -0,0 +1,331 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "imu_display.h"
+
+#include <rviz/properties/status_property.h>
+
+namespace rviz
+{
+
+ImuDisplay::ImuDisplay():
+    fixed_frame_orientation_(true),
+    box_enabled_(false),
+    axes_enabled_(true),
+    acc_enabled_(false),
+    scene_node_(NULL),
+    messages_received_(0)
+{
+    createProperties();
+
+}
+
+void ImuDisplay::onEnable()
+{
+    MessageFilterDisplay<sensor_msgs::Imu>::onEnable();
+
+    if (box_enabled_)
+        box_visual_->show();
+    else
+        box_visual_->hide();
+
+    if (axes_enabled_)
+        axes_visual_->show();
+    else
+        axes_visual_->hide();
+
+    if (acc_enabled_)
+        acc_visual_->show();
+    else
+        acc_visual_->hide();
+}
+
+void ImuDisplay::onDisable()
+{
+    MessageFilterDisplay<sensor_msgs::Imu>::onDisable();
+
+    box_visual_->hide();
+    axes_visual_->hide();
+    acc_visual_->hide();
+}
+
+void ImuDisplay::onInitialize()
+{
+    MFDClass::onInitialize();
+
+    // Make an Ogre::SceneNode to contain all our visuals.
+    scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+
+    // create orientation box visual
+    box_visual_ = new ImuOrientationVisual(
+                context_->getSceneManager(), scene_node_);
+
+    // create orientation axes visual
+    axes_visual_ = new ImuAxesVisual(
+                context_->getSceneManager(), scene_node_);
+
+    // create acceleration vector visual
+    acc_visual_ = new ImuAccVisual(
+                context_->getSceneManager(), scene_node_);
+}
+
+ImuDisplay::~ImuDisplay()
+{
+}
+
+void ImuDisplay::reset()
+{
+    MFDClass::reset();
+    messages_received_ = 0;
+    setStatus(rviz::StatusProperty::Warn, "Topic", "No messages received" );
+
+    box_visual_->hide();
+    axes_visual_->hide();
+    acc_visual_->hide();
+}
+
+void ImuDisplay::updateTop() {
+    fixed_frame_orientation_ = fixed_frame_orientation_property_->getBool();
+}
+
+void ImuDisplay::updateBox() {
+    box_enabled_ = box_enabled_property_->getBool();
+    if (isEnabled() && box_enabled_)
+        box_visual_->show();
+    else
+        box_visual_->hide();
+
+    box_visual_->setScaleX(box_scale_x_property_->getFloat());
+    box_visual_->setScaleY(box_scale_y_property_->getFloat());
+    box_visual_->setScaleZ(box_scale_z_property_->getFloat());
+    box_visual_->setColor(box_color_property_->getColor());
+    box_visual_->setAlpha(box_alpha_property_->getFloat());
+}
+
+void ImuDisplay::updateAxes() {
+    axes_enabled_ = axes_enabled_property_->getBool();
+    if (isEnabled() && axes_enabled_)
+        axes_visual_->show();
+    else
+        axes_visual_->hide();
+
+    axes_visual_->setScale(axes_scale_property_->getFloat());
+
+}
+
+void ImuDisplay::updateAcc() {
+    acc_enabled_ = acc_enabled_property_->getBool();
+    if (isEnabled() && acc_enabled_)
+        acc_visual_->show();
+    else
+        acc_visual_->hide();
+
+    acc_visual_->setScale(acc_scale_property_->getFloat());
+    acc_visual_->setColor(acc_color_property_->getColor());
+    acc_visual_->setAlpha(acc_alpha_property_->getFloat());
+    acc_visual_->setDerotated(acc_derotated_property_->getBool());
+}
+
+void ImuDisplay::processMessage( const sensor_msgs::Imu::ConstPtr& msg )
+{
+    if(!isEnabled())
+        return;
+
+    ++messages_received_;
+
+    std::stringstream ss;
+    ss << messages_received_ << " messages received";
+    setStatus( rviz::StatusProperty::Ok, "Topic", ss.str().c_str() );
+
+    Ogre::Quaternion orientation;
+    Ogre::Vector3 position;
+    if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
+                                                  msg->header.stamp,
+                                                  position, orientation ))
+    {
+        ROS_ERROR("Error transforming from frame '%s' to frame '%s'",
+                  msg->header.frame_id.c_str(), fixed_frame_.toStdString().c_str());
+        return;
+    }
+
+    if (fixed_frame_orientation_) {
+        /* Display IMU orientation is in the world-fixed frame. */
+        Ogre::Vector3 unused;
+        if(!context_->getFrameManager()->getTransform(context_->getFrameManager()->getFixedFrame(),
+                                                      msg->header.stamp,
+                                                      unused, orientation ))
+        {
+            ROS_ERROR("Error getting fixed frame transform");
+            return;
+        }
+    }
+
+    if (box_enabled_)
+    {
+        box_visual_->setMessage(msg);
+        box_visual_->setFramePosition(position);
+        box_visual_->setFrameOrientation(orientation);
+        box_visual_->show();
+    }
+    if (axes_enabled_)
+    {
+        axes_visual_->setMessage(msg);
+        axes_visual_->setFramePosition(position);
+        axes_visual_->setFrameOrientation(orientation);
+        axes_visual_->show();
+    }
+    if (acc_enabled_)
+    {
+        acc_visual_->setMessage(msg);
+        acc_visual_->setFramePosition(position);
+        acc_visual_->setFrameOrientation(orientation);
+        acc_visual_->show();
+    }
+}
+
+void ImuDisplay::createProperties()
+{
+    // **** top level properties
+    fixed_frame_orientation_property_ = new rviz::BoolProperty("fixed_frame_orientation",
+                                                   fixed_frame_orientation_,
+                                                   "Use world fixed frame for display orientation instead of IMU reference frame",
+                                                   this,
+                                                   SLOT(updateTop()),
+                                                   this);
+
+    // **** box properties
+    box_category_ = new rviz::Property("Box properties",
+                                       QVariant(),
+                                       "The list of all the box properties",
+                                       this
+                                       );
+    box_enabled_property_ = new rviz::BoolProperty("Enable box",
+                                                   box_enabled_,
+                                                   "Enable the box display",
+                                                   box_category_,
+                                                   SLOT(updateBox()),
+                                                   this);
+    box_scale_x_property_ = new rviz::FloatProperty("x_scale",
+                                                    1.0,
+                                                    "Box length (x), in meters.",
+                                                    box_category_,
+                                                    SLOT(updateBox()),
+                                                    this);
+    box_scale_y_property_ = new rviz::FloatProperty("y_scale",
+                                                    1.0,
+                                                    "Box length (y), in meters.",
+                                                    box_category_,
+                                                    SLOT(updateBox()),
+                                                    this);
+    box_scale_z_property_ = new rviz::FloatProperty("z_scale",
+                                                    1.0,
+                                                    "Box length (z), in meters.",
+                                                    box_category_,
+                                                    SLOT(updateBox()),
+                                                    this);
+    box_color_property_  = new rviz::ColorProperty("Box color",
+                                                   Qt::red,
+                                                   "Color to draw IMU box",
+                                                   box_category_,
+                                                   SLOT(updateBox()),
+                                                   this);
+    box_alpha_property_ = new rviz::FloatProperty("Box alpha",
+                                                  1.0,
+                                                  "0 is fully transparent, 1.0 is fully opaque.",
+                                                  box_category_,
+                                                  SLOT(updateBox()),
+                                                  this);
+
+    // **** axes properties
+    axes_category_ = new rviz::Property("Axes properties",
+                                       QVariant(),
+                                       "The list of all the axes properties",
+                                       this
+                                       );
+    axes_enabled_property_ = new rviz::BoolProperty("Enable axes",
+                                                   axes_enabled_,
+                                                   "Enable the axes display",
+                                                   axes_category_,
+                                                   SLOT(updateAxes()),
+                                                   this);
+    axes_scale_property_ = new rviz::FloatProperty("Axes scale",
+                                                   true,
+                                                   "Axes size, in meters",
+                                                   axes_category_,
+                                                   SLOT(updateAxes()),
+                                                   this);
+
+    // **** acceleration vector properties
+    acc_category_ = new rviz::Property("Acceleration properties",
+                                       QVariant(),
+                                       "The list of all the acceleration properties",
+                                       this
+                                       );
+    acc_enabled_property_ = new rviz::BoolProperty("Enable acceleration",
+                                                   acc_enabled_,
+                                                   "Enable the acceleration display",
+                                                   acc_category_,
+                                                   SLOT(updateAcc()),
+                                                   this);
+
+    acc_derotated_property_ = new rviz::BoolProperty("Derotate acceleration",
+                                                     true,
+                                                     "If selected, the acceleration is derotated by the IMU orientation. Otherwise, the raw sensor reading is displayed.",
+                                                     acc_category_,
+                                                     SLOT(updateAcc()),
+                                                     this);
+    acc_scale_property_ = new rviz::FloatProperty("Acc. vector scale",
+                                                  true,
+                                                  "Acceleration vector size, in meters",
+                                                  acc_category_,
+                                                  SLOT(updateAcc()),
+                                                  this);
+    acc_color_property_ =  new rviz::ColorProperty("Acc. vector color",
+                                                   Qt::red,
+                                                   "Color to draw acceleration vector.",
+                                                   acc_category_,
+                                                   SLOT(updateAcc()),
+                                                   this);
+    acc_alpha_property_ = new rviz::FloatProperty("Acc. vector alpha",
+                                                 1.0,
+                                                 "0 is fully transparent, 1.0 is fully opaque.",
+                                                 acc_category_,
+                                                 SLOT(updateAcc()),
+                                                  this);
+}
+
+} // end namespace rviz
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(rviz::ImuDisplay, rviz::Display)
+
+

imu_tools/rviz_imu_plugin/src/imu_display.h

@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef RVIZ_IMU_PLUGIN_IMU_DISPLAY_H
+#define RVIZ_IMU_PLUGIN_IMU_DISPLAY_H
+
+#include <message_filters/subscriber.h>
+#include <tf/message_filter.h>
+#include <sensor_msgs/Imu.h>
+#include <rviz/display.h>
+#include <rviz/visualization_manager.h>
+#include <rviz/properties/property.h>
+#include <rviz/frame_manager.h>
+#include <rviz/message_filter_display.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include <tf/transform_listener.h>
+
+#include <rviz/properties/bool_property.h>
+#include <rviz/properties/float_property.h>
+#include <rviz/properties/color_property.h>
+#include <rviz/properties/ros_topic_property.h>
+#include <rviz/display_group.h>
+
+#include "imu_axes_visual.h"
+#include "imu_orientation_visual.h"
+#include "imu_acc_visual.h"
+
+namespace Ogre
+{
+class SceneNode;
+}
+
+namespace rviz
+{
+
+class ImuDisplay: public rviz::MessageFilterDisplay<sensor_msgs::Imu>
+{
+    Q_OBJECT
+public:
+
+    ImuDisplay();
+    virtual ~ImuDisplay();
+
+    virtual void onInitialize();
+    virtual void onEnable();
+    virtual void onDisable();
+    virtual void reset();
+    virtual void createProperties();
+
+    void setTopic(const std::string& topic);
+    const std::string& getTopic() { return topic_; }
+
+    void setBoxEnabled(bool enabled);
+    void setBoxScaleX(float x);
+    void setBoxScaleY(float y);
+    void setBoxScaleZ(float z);
+    void setBoxColor(const Color& color);
+    void setBoxAlpha(float alpha);
+
+    void setAxesEnabled(bool enabled);
+    void setAxesScale(float scale);
+
+    void setAccEnabled(bool enabled);
+    void setAccDerotated(bool derotated);
+    void setAccScale(float scale);
+    void setAccColor(const Color& color);
+    void setAccAlpha(float alpha);
+
+    bool  getBoxEnabled() { return box_enabled_; }
+    float getBoxScaleX()  { return box_visual_->getScaleX(); }
+    float getBoxScaleY()  { return box_visual_->getScaleY(); }
+    float getBoxScaleZ()  { return box_visual_->getScaleZ(); }
+    float getBoxAlpha()   { return box_visual_->getAlpha(); }
+    const QColor& getBoxColor() { return box_visual_->getColor(); }
+
+    bool  getAxesEnabled() { return axes_enabled_; }
+    float getAxesScale()   { return axes_visual_->getScale(); }
+
+    bool  getAccEnabled()   { return acc_enabled_; }
+    float getAccDerotated() { return acc_visual_->getDerotated(); }
+    float getAccScale()     { return acc_visual_->getScale(); }
+    float getAccAlpha()     { return acc_visual_->getAlpha(); }
+    const QColor& getAccColor() { return acc_visual_->getColor(); }
+
+protected Q_SLOTS:
+
+    void updateTop();
+    void updateBox();
+    void updateAxes();
+    void updateAcc();
+
+private:
+
+    // Property objects for user-editable properties.
+    rviz::BoolProperty*  fixed_frame_orientation_property_;
+
+    rviz::Property* box_category_;
+    rviz::Property* axes_category_;
+    rviz::Property* acc_category_;
+
+//    rviz::RosTopicProperty* topic_property_;
+    rviz::BoolProperty*  box_enabled_property_;
+    rviz::FloatProperty* box_scale_x_property_;
+    rviz::FloatProperty* box_scale_y_property_;
+    rviz::FloatProperty* box_scale_z_property_;
+    rviz::ColorProperty* box_color_property_;
+    rviz::FloatProperty* box_alpha_property_;
+
+    rviz::BoolProperty*  axes_enabled_property_;
+    rviz::FloatProperty* axes_scale_property_;
+
+    rviz::BoolProperty*  acc_enabled_property_;
+    rviz::BoolProperty*  acc_derotated_property_;
+    rviz::FloatProperty* acc_scale_property_;
+    rviz::ColorProperty* acc_color_property_;
+    rviz::FloatProperty* acc_alpha_property_;
+
+    // Differetn types of visuals
+    ImuOrientationVisual * box_visual_;
+    ImuAxesVisual        * axes_visual_;
+    ImuAccVisual         * acc_visual_;
+
+    // User-editable property variables.
+    std::string topic_;
+    bool fixed_frame_orientation_;
+    bool box_enabled_;
+    bool axes_enabled_;
+    bool acc_enabled_;
+
+    // A node in the Ogre scene tree to be the parent of all our visuals.
+    Ogre::SceneNode* scene_node_;
+
+    int messages_received_;
+
+    // Function to handle an incoming ROS message.
+    void processMessage( const sensor_msgs::Imu::ConstPtr& msg);
+
+};
+
+} // end namespace rviz
+
+#endif // IMU_DISPLAY_H
+

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "imu_orientation_visual.h"
+
+#include <ros/ros.h>
+#include <cmath>
+
+namespace rviz
+{
+
+ImuOrientationVisual::ImuOrientationVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node):
+  orientation_box_(NULL),
+  scale_x_(0.07),
+  scale_y_(0.10),
+  scale_z_(0.03),
+  alpha_(1.0),
+  quat_valid_(true),
+  color_(0.5, 0.5, 0.5)
+{
+  scene_manager_ = scene_manager;
+
+  // Ogre::SceneNode s form a tree, with each node storing the
+  // transform (position and orientation) of itself relative to its
+  // parent.  Ogre does the math of combining those transforms when it
+  // is time to render.
+  //
+  // Here we create a node to store the pose of the Imu's header frame
+  // relative to the RViz fixed frame.
+  frame_node_ = parent_node->createChildSceneNode();
+}
+
+ImuOrientationVisual::~ImuOrientationVisual()
+{
+  hide();
+
+  // Destroy the frame node since we don't need it anymore.
+  scene_manager_->destroySceneNode(frame_node_);
+}
+
+void ImuOrientationVisual::show()
+{
+  if (!orientation_box_)
+  {
+    orientation_box_ = new Shape(Shape::Cube, scene_manager_, frame_node_);
+    orientation_box_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
+    orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
+    orientation_box_->setOrientation(orientation_);
+  }
+}
+
+void ImuOrientationVisual::hide()
+{
+  if (orientation_box_)
+  {
+    delete orientation_box_;
+    orientation_box_ = NULL;
+  }
+}
+
+void ImuOrientationVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg)
+{
+  if (checkQuaternionValidity(msg)) {
+    if (!quat_valid_) {
+      ROS_INFO("rviz_imu_plugin got valid quaternion, "
+               "displaying true orientation");
+      quat_valid_ = true;
+    }
+    orientation_ = Ogre::Quaternion(msg->orientation.w,
+                                    msg->orientation.x,
+                                    msg->orientation.y,
+                                    msg->orientation.z);
+  } else {
+    if (quat_valid_) {
+      ROS_WARN("rviz_imu_plugin got invalid quaternion (%lf, %lf, %lf, %lf), "
+               "will display neutral orientation instead", msg->orientation.w,
+               msg->orientation.x,msg->orientation.y,msg->orientation.z);
+      quat_valid_ = false;
+    }
+    // if quaternion is invalid, give a unit quat to Ogre
+    orientation_ = Ogre::Quaternion();
+  }
+
+  if (orientation_box_)
+    orientation_box_->setOrientation(orientation_);
+}
+
+void ImuOrientationVisual::setScaleX(float x) 
+{ 
+  scale_x_ = x; 
+  if (orientation_box_) 
+   orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
+}
+
+void ImuOrientationVisual::setScaleY(float y) 
+{ 
+  scale_y_ = y; 
+  if (orientation_box_) 
+    orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
+}
+
+void ImuOrientationVisual::setScaleZ(float z) 
+{ 
+  scale_z_ = z; 
+  if (orientation_box_) 
+    orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
+
+}
+
+void ImuOrientationVisual::setColor(const QColor& color)
+{
+  color_ = color;
+  if (orientation_box_) 
+    orientation_box_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
+}
+
+void ImuOrientationVisual::setAlpha(float alpha) 
+{ 
+  alpha_ = alpha; 
+  if (orientation_box_) 
+    orientation_box_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
+}
+
+void ImuOrientationVisual::setFramePosition(const Ogre::Vector3& position)
+{
+  frame_node_->setPosition(position);
+}
+
+void ImuOrientationVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
+{
+  frame_node_->setOrientation(orientation);
+}
+
+inline bool ImuOrientationVisual::checkQuaternionValidity(
+    const sensor_msgs::Imu::ConstPtr& msg) {
+
+  double x = msg->orientation.x,
+         y = msg->orientation.y,
+         z = msg->orientation.z,
+         w = msg->orientation.w;
+  // OGRE can handle unnormalized quaternions, but quat's length extremely small;
+  // this may indicate that invalid (0, 0, 0, 0) quat is passed, this will lead ogre
+  // to crash unexpectly
+  if ( std::sqrt( x*x + y*y + z*z + w*w ) < 0.0001 ) {
+    return false;
+  }
+  return true;
+}
+
+
+} // end namespace rviz
+

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.h

@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H
+#define RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H
+
+#include <sensor_msgs/Imu.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include <rviz/ogre_helpers/shape.h>
+#include <rviz/helpers/color.h>
+#include <QColor>
+
+namespace rviz
+{
+
+class ImuOrientationVisual
+{
+  public:
+    // Constructor.  Creates the visual stuff and puts it into the
+    // scene, but in an unconfigured state.
+    ImuOrientationVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
+
+    // Destructor.  Removes the visual stuff from the scene.
+    virtual ~ImuOrientationVisual();
+
+    // Configure the visual to show the data in the message.
+    void setMessage(const sensor_msgs::Imu::ConstPtr& msg);
+
+    // Set the pose of the coordinate frame the message refers to.
+    // These could be done inside setMessage(), but that would require
+    // calls to FrameManager and error handling inside setMessage(),
+    // which doesn't seem as clean.  This way ImuVisual is only
+    // responsible for visualization.
+    void setFramePosition(const Ogre::Vector3& position);
+    void setFrameOrientation(const Ogre::Quaternion& orientation);
+
+    // Set the color and alpha of the visual, which are user-editable
+
+    void setScaleX(float x);
+    void setScaleY(float y);
+    void setScaleZ(float z);
+    void setColor(const QColor &color);
+    void setAlpha(float alpha);
+
+    float getScaleX() { return scale_x_; }
+    float getScaleY() { return scale_y_; }
+    float getScaleZ() { return scale_z_; }
+    const QColor& getColor() { return color_; }
+    float getAlpha() { return alpha_; }
+
+    void show();
+    void hide();
+
+  private:
+
+    void create();
+    inline bool checkQuaternionValidity(
+        const sensor_msgs::Imu::ConstPtr& msg);
+
+    Ogre::Quaternion orientation_;
+
+    float scale_x_, scale_y_, scale_z_;
+    QColor color_;
+    float alpha_;
+    bool quat_valid_;
+
+    Shape * orientation_box_;
+  
+    // A SceneNode whose pose is set to match the coordinate frame of
+    // the Imu message header.
+    Ogre::SceneNode * frame_node_;
+
+    // The SceneManager, kept here only so the destructor can ask it to
+    // destroy the ``frame_node_``.
+    Ogre::SceneManager * scene_manager_;
+};
+
+} // end namespace rviz
+
+#endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H

rviz_display_6dof_imu/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rviz_display_6dof_imu)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES rviz_display_6dof_imu
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/rviz_display_6dof_imu.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/rviz_display_6dof_imu_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_rviz_display_6dof_imu.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rviz_display_6dof_imu/cfg/6dof_imu_display.rviz

@@ -0,0 +1,143 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 123
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1/Offset1
+      Splitter Ratio: 0.6222222447395325
+    Tree Height: 344
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.699999988079071
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 50; 115; 54
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration properties:
+        Acc. vector alpha: 0.800000011920929
+        Acc. vector color: 179; 200; 255
+        Acc. vector scale: 0.05000000074505806
+        Derotate acceleration: false
+        Enable acceleration: false
+      Axes properties:
+        Axes scale: 1
+        Enable axes: true
+      Box properties:
+        Box alpha: 0.6000000238418579
+        Box color: 255; 0; 0
+        Enable box: true
+        x_scale: 0.10000000149011612
+        y_scale: 0.10000000149011612
+        z_scale: 0.10000000149011612
+      Class: rviz_imu_plugin/Imu
+      Enabled: true
+      Name: Imu
+      Topic: /imu_data
+      Unreliable: false
+      Value: true
+      fixed_frame_orientation: true
+  Enabled: true
+  Global Options:
+    Background Color: 45; 32; 41
+    Default Light: true
+    Fixed Frame: base_imu_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.9545544385910034
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.026923831552267075
+        Y: 0.048061929643154144
+        Z: 0.06090698018670082
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4303995668888092
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.730486273765564
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 714
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000021bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000440000021b000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000040000000044fc0100000002fb0000000800540069006d00650100000000000004000000028000fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000021b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1024
+  X: 0
+  Y: 96

rviz_display_6dof_imu/launch/rviz_display_6dof_imu.launch

@@ -0,0 +1,12 @@
+<!---
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  Description:该launch启动文件是为了可以在rviz中可视化查看imu当前的姿态信息.
+  Author: corvin
+  History:
+    20211122:Initial this launch file.
+-->
+<launch>
+  <node pkg="rviz" type="rviz" name="imu_rviz_node"
+        args="-d $(find rviz_display_6dof_imu)/cfg/6dof_imu_display.rviz">
+  </node>
+</launch>

rviz_display_6dof_imu/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rviz_display_6dof_imu</name>
+  <version>0.0.0</version>
+  <description>The rviz_display_6dof_imu package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rviz_display_6dof_imu</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

serial_6dof_imu/CMakeLists.txt

@@ -0,0 +1,193 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(serial_6dof_imu)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  getYawData.srv
+  setBaudRate.srv
+  setYawZero.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/serial_mode_imu.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(serial_6dof_imu_node src/serial_imu_node.cpp src/proc_serial_data.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(serial_6dof_imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(serial_6dof_imu_node
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_serial_mode_imu.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

serial_6dof_imu/cfg/param.yaml

@@ -0,0 +1,38 @@
+###########################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:使用串口与IMU模块进行通信时的配置信息,根据需要修改
+# 可以配置的各参数如下(这里需要注意参数名称和参数之间要有空格隔开):
+#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
+#     模块进行通信,都是写ttyUSB0,1,2.
+#  baud_rate:串口通信波特率,默认为115200,如果查看发布的
+#  　　话题中没有IMU数据可以尝试换波特率为9600或57600或115200.
+#  pub_topic_hz:话题发布的数据频率,默认设置为100Hz.
+#  imu_link_name:imu模块的link名,该名称一般在urdf模型中定义.
+#  pub_data_topic:发布imu数据的ROS话题名.
+#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pitch_pub_topic:将pitch数据通过该话题发布出来.
+#  roll_pub_topic:将roll数据通过该话题发布出来.
+#  yaw_zero_topic:通过向该话题发布空消息即可将yaw角度归零.
+#  yaw_zero_service:通过向该服务发送空请求即可将yaw归零.
+#  baud_update_srv:通过向该服务发送1,2,3可以将波特率更新为
+#    9600bps,57600bps,115200bps,三种通信波特率.
+#  yaw_data_srv:可以获取到yaw当前角度的服务.
+# Author: corvin
+# History:
+#   20211122:init this file.
+###########################################################
+imu_dev: /dev/ttyUSB0
+baud_rate: 115200
+pub_topic_hz: 100
+
+imu_link_name: base_imu_link
+pub_data_topic: imu_data
+yaw_pub_topic: yaw_data
+pitch_pub_topic: pitch_data
+roll_pub_topic: roll_data
+yaw_zero_topic: yaw_zero_topic
+
+yaw_zero_service: /serial_imu_service/setYawZero_service
+baud_update_srv: /serial_imu_service/updateBaud_service
+yaw_data_srv: /serial_imu_service/getYawData_service

serial_6dof_imu/include/imu_data.h

@@ -0,0 +1,26 @@
+/*******************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口操作读取imu模块的代码头文件.
+ * Author: corvin
+ * History:
+ *   20210318:init this file.
+ *   20210319:增加修改串口通信波特率的函数.
+ *   20210321:增加控制IMU引脚数字高低电平的函数.
+ *******************************************************/
+#ifndef _IMU_DATA_H_
+#define _IMU_DATA_H_
+
+int initSerialPort(const char* path, const int baud);
+
+int getImuData(void);
+int closeSerialPort(void);
+
+float getAcc(int flag);
+float getAngular(int flag);
+float getAngle(int flag);
+float getQuat(int flag);
+
+int makeYawZero(void);
+int setIMUBaudRate(const int flag);
+int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3);
+#endif

serial_6dof_imu/launch/serial_6dof_imu.launch

@@ -0,0 +1,13 @@
+<!--
+  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+  Description:使用串口来读取6DOF imu模块的数据,并在ros中发布使用话题发布出来.
+  Author: corvin
+  History:
+    20211122: init this file.
+-->
+<launch>
+  <arg name="serial_6dof_imu_cfg" default="$(find serial_6dof_imu)/cfg/param.yaml" />
+  <node pkg="serial_6dof_imu" type="serial_6dof_imu_node" name="serial_imu_node" output="screen">
+    <rosparam file="$(arg serial_6dof_imu_cfg)" command="load" />
+  </node>
+</launch>

serial_6dof_imu/package.xml

@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>serial_6dof_imu</name>
+  <version>0.0.0</version>
+  <description>The serial_6dof_imu package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/serial_mode_imu</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

serial_6dof_imu/src/proc_serial_data.cpp

@@ -0,0 +1,388 @@
+/*******************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口方式读取和控制IMU模块信息.
+ * Author: corvin
+ * History:
+ *   20211122:init this file.
+******************************************************************/
+#include<ros/ros.h>
+#include<stdio.h>
+#include<stdlib.h>
+#include<fcntl.h>
+#include<unistd.h>
+#include<termios.h>
+#include<string.h>
+#include<sys/time.h>
+#include<sys/types.h>
+
+#define  BYTE_CNT      88  //每次从串口中读取的字节数
+#define  ACCE_CONST    0.000488281   //用于计算加速度的常量16.0/32768.0
+#define  ANGULAR_CONST 0.061035156   //用于计算角速度的常量2000.0/32768.0
+#define  ANGLE_CONST   0.005493164   //用于计算欧拉角的常量180.0/32768.0
+
+static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
+static int port_fd = -1; //串口打开时的文件描述符
+static float acce[3],angular[3],angle[3],quater[4];
+
+/******************************************************
+ * Description:打开IMU模块串口,输入两个参数即可连接.
+ *   open()打开失败时,返回-1,成功打开时返回文件描述符.
+ *   各参数意义如下:
+ *   const char *path:IMU设备的挂载地址;
+ *   const int baud:串口通讯的波特率;
+ *****************************************************/
+int openSerialPort(const char *path, const int baud)
+{
+    int fd = 0;
+    struct termios terminfo;
+    bzero(&terminfo, sizeof(terminfo));
+
+    fd = open(path, O_RDWR|O_NOCTTY);
+    if (-1 == fd)
+    {
+        ROS_ERROR("Open 6DOF IMU dev error:%s, baud:%d", path, baud);
+        return -1;
+    }
+
+    //判断当前连接的设备是否为终端设备
+    if (isatty(STDIN_FILENO) == 0)
+    {
+        ROS_ERROR("IMU dev isatty error !");
+        return -2;
+    }
+
+    /*设置串口通信波特率-bps*/
+    switch(baud)
+    {
+        case 9600:
+           cfsetispeed(&terminfo, B9600); //设置输入速度
+           cfsetospeed(&terminfo, B9600); //设置输出速度
+           break;
+
+        case 57600:
+           cfsetispeed(&terminfo, B57600);
+           cfsetospeed(&terminfo, B57600);
+           break;
+
+       case 115200:
+           cfsetispeed(&terminfo, B115200);
+           cfsetospeed(&terminfo, B115200);
+           break;
+
+       default: //设置默认波特率9600
+           cfsetispeed(&terminfo, B9600);
+           cfsetospeed(&terminfo, B9600);
+           break;
+    }
+
+    //设置数据位 - 8 bit
+    terminfo.c_cflag |= CLOCAL|CREAD;
+    terminfo.c_cflag &= ~CSIZE;
+    terminfo.c_cflag |= CS8;
+
+    //不设置奇偶校验位 - N
+    terminfo.c_cflag &= ~PARENB;
+
+    //设置停止位 - 1
+    terminfo.c_cflag &= ~CSTOPB;
+
+    //设置等待时间和最小接收字符
+    terminfo.c_cc[VTIME] = 0;
+    terminfo.c_cc[VMIN]  = 1;
+
+    //清除串口缓存的数据
+    tcflush(fd, TCIFLUSH);
+
+    if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
+    {
+        ROS_ERROR("set imu serial port attr error !");
+        return -3;
+    }
+
+    return fd;
+}
+
+/**************************************
+ * Description:关闭串口文件描述符.
+ *************************************/
+int closeSerialPort(void)
+{
+    int ret = close(port_fd);
+    return ret;
+}
+
+/*****************************************************************
+ * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
+ *   发送控制命令,最后就是需要发送命令保存刚才的操作.解锁命令是固
+ *   定的0xFF 0xAA 0x69 0x88 0xB5,保存命令也是固定的0xFF 0xAA 0x00
+ *   0x00 0x00.
+ *****************************************************************/
+int send_data(int fd, unsigned char *send_buffer, int length)
+{
+    int ret = -1;
+    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
+    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+
+    ret = write(fd, unLockCmd, sizeof(unLockCmd));
+    if(ret != sizeof(unLockCmd))
+    {
+        ROS_ERROR("Send IMU module unlock cmd error !");
+        return -1;
+    }
+    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
+
+    #if 0 //打印发送给IMU模块的数据,用于调试
+    printf("Send %d byte to IMU:", length);
+    for(int i=0; i<length; i++)
+    {
+        printf("0x%02x ", send_buffer[i]);
+    }
+    printf("\n");
+    #endif
+
+    //发送控制命令
+    ret = write(fd, send_buffer, length*sizeof(unsigned char));
+    if(ret != length)
+    {
+        ROS_ERROR("Send IMU module control cmd error !");
+        return -2;
+    }
+
+    //发送保存命令
+    ret = write(fd, saveCmd, sizeof(saveCmd));
+    if(ret != sizeof(saveCmd))
+    {
+        ROS_ERROR("Send IMU module save cmd error !");
+        return -3;
+    }
+    ros::Duration(0.1).sleep(); //delay 100ms才能进行其他操作
+
+    return 0;
+}
+
+/**************************************************************
+ * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
+ *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+void parse_serialData(unsigned char chr)
+{
+    static unsigned char chrBuf[BYTE_CNT];
+    static unsigned char chrCnt = 0;  //记录读取的第几个字节
+
+    signed short sData[4]; //save 8 Byte有效信息
+    unsigned char i = 0;   //用于for循环
+    unsigned char frameSum = 0;  //存储数据帧的校验和
+
+    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
+
+    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
+    if(chrCnt < 11)
+        return;
+
+    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
+    for(i=0; i<10; i++)
+    {
+        frameSum += chrBuf[i];
+    }
+
+    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
+    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
+    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
+    {
+        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
+        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
+        return;
+    }
+
+    #if 0 //打印出完整的带帧头尾的数据帧
+    for(i=0; i<11; i++)
+      printf("0x%02x ",chrBuf[i]);
+    printf("\n");
+    #endif
+
+    memcpy(&sData[0], &chrBuf[2], 8);
+    switch(chrBuf[1])  //根据数据帧不同类型来进行解析数据
+    {
+        case 0x51: //x,y,z轴 加速度输出
+            acce[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ACCE_CONST;
+            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ACCE_CONST;
+            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ACCE_CONST;
+            break;
+        case 0x52: //角速度输出
+            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGULAR_CONST;
+            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGULAR_CONST;
+            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGULAR_CONST;
+            break;
+        case 0x53: //欧拉角度输出, roll, pitch, yaw
+            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGLE_CONST;
+            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGLE_CONST;
+            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGLE_CONST;
+            break;
+        case 0x59: //四元素输出
+            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
+            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
+            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
+            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
+            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
+            break;
+        default:
+            ROS_ERROR("parse imu data frame type error !");
+            break;
+    }
+    chrCnt = 0;
+}
+
+/********************************************************************
+ * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
+ *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
+ *******************************************************************/
+int makeYawZero(void)
+{
+    int ret = 0;
+    unsigned char yawZeroCmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
+
+    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
+    if(ret != 0) //通过串口发送命令失败
+    {
+        ROS_ERROR("Send yaw zero control cmd error !");
+        return -1;
+    }
+    ROS_INFO("set yaw to zero radian successfully !");
+
+    return 0;
+}
+
+/******************************************************************
+ * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的
+ *   波特率,1修改为9600,2修改为57600,3修改为115200.修改波特率就是
+ *   发送3条指令,第一条命令是解除锁定,然后是发送串口波特率更新命令,
+ *   最后是保存操作的指令.
+ ******************************************************************/
+int setIMUBaudRate(const int flag)
+{
+    int ret = 0;
+    int baud = 0;
+    unsigned char baudRate[5] = {0xFF, 0xAA, 0x04, 0x00, 0x00};
+
+    switch(flag)
+    {
+        case 1: //set baud 9600 bps
+            baudRate[3] = 0x02;
+            baud = 9600;
+            break;
+        case 2: //set baud 57600 bps
+            baudRate[3] = 0x05;
+            baud = 57600;
+            break;
+        case 3: //set baud 115200 bps
+            baudRate[3] = 0x06;
+            baud = 115200;
+            break;
+        default: //default set baud 57600 bps
+            baudRate[3] = 0x05;
+            baud = 57600;
+            break;
+    }
+    ret = send_data(port_fd, baudRate, sizeof(baudRate));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send baud rate update cmd error !");
+        return -1;
+    }
+    ROS_INFO("Set imu new baud rate:[ %d bps] successfully !", baud);
+    ROS_INFO("Please reconnect IMU module with new baud !");
+
+    return 0;
+}
+
+/*****************************************************************
+ * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
+ *   两个输入参数的意义如下:
+ *   const char* path:IMU设备的挂载地址;
+ *   const int baud:IMU模块的串口通信波特率;
+ ****************************************************************/
+int initSerialPort(const char* path, const int baud)
+{
+    port_fd = openSerialPort(path, baud);
+    if(port_fd < 0) //打开IMU模块串口失败
+    {
+        ROS_ERROR("Open 6DOF IMU Module Serial Port error !");
+        return -1;
+    }
+
+    return 0;
+}
+
+/*****************************************
+ * Description:得到三轴加速度信息,输入
+ * 　参数可以为0,1,2分别代表x,y,z轴的
+ * 　加速度信息.
+ *****************************************/
+float getAcc(int flag)
+{
+    return acce[flag];
+}
+
+/******************************************
+ * Description:得到角速度信息,输入参数可
+ * 　以为0,1,2,分别代表x,y,z三轴的角速度
+ *   信息.
+ *****************************************/
+float getAngular(int flag)
+{
+    return angular[flag];
+}
+
+/********************************************
+ * Description:获取yaw,pitch,roll,输入参数0,
+ * 1,2可以分别获取到roll,pitch,yaw的数据.
+ *******************************************/
+float getAngle(int flag)
+{
+    return angle[flag];
+}
+
+/********************************************
+ * Description:输入参数0,1,2,3可以分别获取到
+ * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
+ * 　的imu_msg.orientation.w,x,y,z的顺序,
+ * 　这里的q0是对应w参数,1,2,3对应x,y,z.
+ ********************************************/
+float getQuat(int flag)
+{
+    return quater[flag];
+}
+
+/*************************************************************
+ * Description:从串口读取数据,然后解析出各有效数据段,这里
+ *  一次性从串口中读取88个字节,然后需要从这些字节中进行
+ *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+int getImuData(void)
+{
+    int data_len = 0;
+    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
+
+    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
+    data_len = read(port_fd, r_buf, sizeof(r_buf));
+    if(data_len <= 0) //从串口中读取数据失败
+    {
+        ROS_ERROR("read serial port data failed !\n");
+        closeSerialPort();
+        return -1;
+    }
+
+    //printf("recv data len:%d\n", data_len); //一次性从串口中读取的总数据字节数
+    for (int i=0; i<data_len; i++) //将读取到的数据进行解析
+    {
+        //printf("0x%02x ", r_buf[i]);
+        parse_serialData(r_buf[i]);
+    }
+    //printf("\n\n");
+
+    return 0;
+}

serial_6dof_imu/src/serial_imu_node.cpp

@@ -0,0 +1,186 @@
+/*****************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU的数据,并通过ROS话题topic将数据发布出来.
+ *   芯片中默认读取出来的加速度数据单位是g,需要将其转换为ROS中加速度规定的
+ *   m/s2才能发布.
+ * Author: corvin
+ * History:
+ *   20211122: init this file.
+*****************************************************************/
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <imu_data.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Empty.h>
+#include "serial_6dof_imu/setYawZero.h"
+#include "serial_6dof_imu/setBaudRate.h"
+#include "serial_6dof_imu/getYawData.h"
+
+
+static float g_yawData;  //全局变量,存储当前yaw值,可以通过服务来得到该值
+
+/**********************************************************
+ * Description:将yaw角度归零的话题回调函数,只要往话题中
+ *   发布一条Empty消息,即可将yaw角度归零.
+ *********************************************************/
+void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
+{
+    makeYawZero();
+}
+
+/******************************************************************
+ * Description:使用service方式来进行yaw角度归零,这里是服务的回调
+ *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
+ *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
+ *   命令执行失败.
+ *****************************************************************/
+bool yawZeroService(serial_6dof_imu::setYawZero::Request &req,
+                    serial_6dof_imu::setYawZero::Response &res)
+{
+    res.status = makeYawZero();
+    return true;
+}
+
+/********************************************************************
+ * Description:使用服务调用方式来修改与IMU模块通信的波特率,这里根据
+ *   客户端发送过来的request来决定修改的波特率,请求内容对应的波特率:
+ *   请求发送1,修改波特率为9600;
+ *   请求发送2,修改波特率为57600;
+ *   请求发送3,修改波特率为115200;
+ *   当修改IMU模块的波特率成功后函数会返回0,若返回其他负值,则表示
+ *   修改波特率失败,可以重新调用服务来修改波特率.
+ *******************************************************************/
+bool setBaudService(serial_6dof_imu::setBaudRate::Request &req,
+                    serial_6dof_imu::setBaudRate::Response &res)
+{
+    res.status = setIMUBaudRate(req.index);
+    return true;
+}
+
+/**********************************************************************
+ * Description:通过service服务调用方式来获取到yaw角度信息.
+ **********************************************************************/
+bool getYawDataService(serial_6dof_imu::getYawData::Request &req,
+                       serial_6dof_imu::getYawData::Response &res)
+{
+    res.yaw = g_yawData;
+    return true;
+}
+
+int main(int argc, char **argv)
+{
+    float yaw, pitch, roll;
+    std::string imu_dev;
+    int baud = 0;
+
+    std::string imu_link_name;
+    std::string imu_topic_name;
+    std::string yaw_pub_topic;
+    std::string pitch_pub_topic;
+    std::string roll_pub_topic;
+    std::string yaw_zero_topic;
+    std::string yaw_zero_service;
+    std::string baud_update_service;
+    std::string yaw_data_service;
+
+    int pub_topic_hz = 0;  //话题发布imu数据的频率
+    float degree2Rad = 3.1415926/180.0;
+    float acc_factor = 9.806;
+
+    ros::init(argc, argv, "imu_data_pub_node");
+    ros::NodeHandle handle;
+
+    //launch文件中加载yaml配置文件，然后从配置文件中读取参数
+    ros::param::get("~imu_dev",          imu_dev);
+    ros::param::get("~baud_rate",        baud);
+    ros::param::get("~imu_link_name",    imu_link_name);
+    ros::param::get("~pub_topic_hz",     pub_topic_hz);
+    ros::param::get("~pub_data_topic",   imu_topic_name);
+    ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
+    ros::param::get("~yaw_zero_service", yaw_zero_service);
+    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
+    ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
+    ros::param::get("~roll_pub_topic",   roll_pub_topic);
+    ros::param::get("~baud_update_srv",  baud_update_service);
+    ros::param::get("~yaw_data_srv",     yaw_data_service);
+
+    //初始化imu模块串口,根据设备号和波特率建立连接
+    int ret = initSerialPort(imu_dev.c_str(), baud);
+    if(ret < 0) //通过串口连接IMU模块失败
+    {
+        ROS_ERROR("init 6DOF IMU module serial port error !");
+        closeSerialPort();
+        return -1;
+    }
+    ROS_INFO("Now 6DOF IMU module is working...");
+
+    //定义四个服务,分别是更新波特率,yaw角度归零和获取yaw当前角度
+    ros::ServiceServer setBaudSrv = handle.advertiseService(baud_update_service, setBaudService);
+    ros::ServiceServer setyawSrv  = handle.advertiseService(yaw_zero_service,  yawZeroService);
+    ros::ServiceServer getYawSrv  = handle.advertiseService(yaw_data_service,  getYawDataService);
+
+    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
+    ros::Publisher imu_pub   = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);
+    ros::Publisher yaw_pub   = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 2);
+    ros::Publisher pitch_pub = handle.advertise<std_msgs::Float32>(pitch_pub_topic, 2);
+    ros::Publisher roll_pub  = handle.advertise<std_msgs::Float32>(roll_pub_topic, 2);
+    ros::Rate loop_rate(pub_topic_hz);
+
+    sensor_msgs::Imu imu_msg;
+    imu_msg.header.frame_id = imu_link_name;
+    std_msgs::Float32 yaw_msg;
+    std_msgs::Float32 pitch_msg;
+    std_msgs::Float32 roll_msg;
+    while(ros::ok())
+    {
+        if(getImuData() < 0)
+            break;
+
+        imu_msg.header.stamp = ros::Time::now();
+        roll  = getAngle(0)*degree2Rad;
+        pitch = getAngle(1)*degree2Rad;
+        yaw   = getAngle(2)*degree2Rad;
+        if(yaw >= 3.1415926)
+            yaw -= 6.2831852;
+
+        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
+        yaw_msg.data   = yaw;
+        pitch_msg.data = pitch;
+        roll_msg.data  = roll;
+        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
+        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
+        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
+
+        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        imu_msg.orientation.x = getQuat(1);
+        imu_msg.orientation.y = getQuat(2);
+        imu_msg.orientation.z = getQuat(3);
+        imu_msg.orientation.w = getQuat(0);
+        imu_msg.orientation_covariance = {0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025};
+
+        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
+        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
+        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
+        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
+                                               0, 0.02, 0,
+                                               0, 0, 0.02};
+
+        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAcc(0), getAcc(1), getAcc(2));
+        imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
+        imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
+        imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
+        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
+                                                  0, 0.04, 0,
+                                                  0, 0, 0.04};
+
+        imu_pub.publish(imu_msg); //将imu数据通过话题发布出去
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    closeSerialPort(); //关闭与IMU模块的串口连接
+    return 0;
+}

serial_6dof_imu/srv/getYawData.srv

@@ -0,0 +1,2 @@
+---
+float32 yaw

serial_6dof_imu/srv/setBaudRate.srv

@@ -0,0 +1,3 @@
+int32 index
+---
+int32 status

serial_6dof_imu/srv/setYawZero.srv

@@ -0,0 +1,2 @@
+---
+int32 status