将当前main分支代码更新为ROS2 humble下可以直接运行使用的代码，然后对于原来main代码，移动到ros1分支上

.gitignore

@@ -13,6 +13,8 @@ __pycache__/
 .Python
 env/
 build/
+install/
+log/
 develop-eggs/
 dist/
 downloads/
@@ -59,4 +61,3 @@ docs/_build/
 
 # PyBuilder
 target/
-

.gitignore.orig

@@ -1,128 +0,0 @@
-<<<<<<< HEAD
-# ---> C++
-# Compiled Object files
-*.slo
-*.lo
-*.o
-*.obj
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Compiled Dynamic libraries
-*.so
-*.dylib
-*.dll
-
-# Fortran module files
-*.mod
-
-# Compiled Static libraries
-*.lai
-*.la
-*.a
-*.lib
-
-# Executables
-*.exe
-*.out
-*.app
-
-# ---> C
-# Object files
-*.o
-*.ko
-*.obj
-*.elf
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Libraries
-*.lib
-*.a
-*.la
-*.lo
-
-# Shared objects (inc. Windows DLLs)
-*.dll
-*.so
-*.so.*
-*.dylib
-
-# Executables
-*.exe
-*.out
-*.app
-*.i*86
-*.x86_64
-*.hex
-
-# Debug files
-*.dSYM/
-=======
-# ---> Python
-# Byte-compiled / optimized / DLL files
-__pycache__/
-*.py[cod]
-*$py.class
-
-.vscode/
-
-# 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/
->>>>>>> origin/master
-

README.md

@@ -1,8 +1,65 @@
 ### 代码仓库介绍
 
-- 该代码为mini 6dof imu模块配套的代码仓库，代码仓库共有6个分支，当前代码分支为master分支，为ROS系统配套代码分支
-- 剩下代码分支依次为:arduino-dev,linux-dev,matlab-dev,stm32-dev,windows-dev
-- 当前master分支代码ROS配套代码，支持ROS kinetic,melodic,noetic常用版本
-- 使用该代码时需要将其放在ROS工作区下的src目录中,然后在工作区根目录执行catkin_make命令就可以编译了
-- 当编译好代码运行imu模块前,需要先执行下当前目录下的initdev_mini_imu.sh脚本,`sudo ./initdev_mini_imu.sh`
+该仓库为 mini 6DOF IMU 模块的 ROS2 Humble 驱动代码，包含两种读取方式：
 
+- `serial_6dof_imu`：通过串口读取 IMU 数据，发布 `sensor_msgs/msg/Imu`、yaw/pitch/roll/temp 话题，并提供 yaw 归零、读取 yaw、设置 IIC 地址服务。
+- `iic_6dof_imu`：通过 IIC 读取 IMU 数据，发布 `sensor_msgs/msg/Imu`、yaw/pitch/roll 话题，并提供读取 yaw 服务。
+- `rviz_display_6dof_imu`：启动 RViz2 加载 IMU 可视化配置。
+
+### 编译
+
+将仓库放在 ROS2 工作区的 `src` 目录下：
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-select serial_6dof_imu iic_6dof_imu rviz_display_6dof_imu
+source install/setup.bash
+```
+
+### 串口设备映射
+
+运行 IMU 前可执行脚本将 USB 串口映射为 `/dev/mini_imu`：
+
+```bash
+sudo ./initdev_mini_imu.sh
+```
+
+### 启动
+
+串口方式：
+
+```bash
+ros2 launch serial_6dof_imu serial_6dof_imu.launch.py
+```
+
+IIC 方式：
+
+```bash
+sudo apt install python3-smbus
+ros2 launch iic_6dof_imu iic_6dof_imu.launch.py
+```
+
+RViz2 显示：
+
+```bash
+ros2 launch rviz_display_6dof_imu rviz_display_6dof_imu.launch.py
+```
+
+RViz2 的 IMU 显示依赖 ROS2 版 `rviz_imu_plugin`。如果系统未安装，可先安装：
+
+```bash
+sudo apt install ros-humble-imu-tools
+```
+
+### 常用服务
+
+```bash
+ros2 service call /serial_imu_service/setYawZero_service serial_6dof_imu/srv/SetYawZero
+ros2 service call /serial_imu_service/getYawData_service serial_6dof_imu/srv/GetYawData
+ros2 service call /serial_imu_service/setIICAddr_service serial_6dof_imu/srv/SetIICAddr "{address: '0x50'}"
+ros2 service call /iic_imu_service/getYawData_service iic_6dof_imu/srv/GetYawData
+```
+
+### 说明
+
+旧 ROS1 第三方源码 `imu_tools/` 已从仓库中移除。Humble 环境请直接使用官方发布的 `ros-humble-imu-tools`。

README.md.orig

@@ -1,18 +0,0 @@
-<<<<<<< HEAD
-# mini_6dof_imu
-
-该代码为mini 6dof imu模块配套的代码仓库，代码仓库共有6个分支，当前代码分支为master分支，为ROS系统配套代码分支
-剩下代码分支依次为:arduino-dev,linux-dev,matlab-dev,stm32-dev,windows-dev
-当前master分支代码ROS配套代码，支持ROS kinetic,melodic,noetic常用版本
-使用该代码时需要将其放在ROS工作区下的src目录中,然后在工作区根目录执行catkin_make命令就可以编译了
-当编译好代码运行imu模块前,需要先执行下当前目录下的initdev_mini_imu.sh脚本,sudo ./initdev_mini_imu.sh
-=======
-### 代码仓库介绍
-
-- 该代码为mini 6dof imu模块配套的代码仓库，代码仓库共有6个分支，当前代码分支为master分支，为ROS系统配套代码分支
-- 剩下代码分支依次为:arduino-dev,linux-dev,matlab-dev,stm32-dev,windows-dev
-- 当前master分支代码ROS配套代码，支持ROS kinetic,melodic,noetic常用版本
-- 使用该代码时需要将其放在ROS工作区下的src目录中,然后在工作区根目录执行catkin_make命令就可以编译了
-- 当编译好代码运行imu模块前,需要先执行下当前目录下的initdev_mini_imu.sh脚本,`sudo ./initdev_mini_imu.sh`
-
->>>>>>> origin/master

iic_6dof_imu/CMakeLists.txt

@@ -1,199 +1,29 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(iic_6dof_imu)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-## 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
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetYawData.srv"
 )
 
-## 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
+install(PROGRAMS
+  src/iic_6dof_imu_node.py
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  sensor_msgs
-  std_msgs
+install(FILES
+  src/iic_6dof_imu_data.py
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-################################################
-## 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}
+install(DIRECTORY cfg launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-## 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)
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

iic_6dof_imu/cfg/param.yaml

@@ -1,28 +1,16 @@
 ###################################################
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用IIC总线来读取IMU模块的数据,并
-#   通过话题将imu数据发送出来.这里是可以配置的一些参数,
-#   各参数意义如下:
-#   iic_bus:当前使用的设备上哪一条iic总线,默认1.
-#   iic_addr:当前IMU板的IIC地址,默认0x50.
-#   topic_pub_hz:上述各话题发布的频率,默认100hz.
-#   pub_data_topic:发布imu数据的话题名.
-#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
-#   pitch_topic:发布俯仰角的话题名,单位弧度.
-#   roll_topic:发布横滚角的话题名,单位弧度.
-#   link_name:imu模块的urdf中link名字.
-#   get_yaw_service:可以获取到yaw角度信息的service名称.
-# Author: corvin
-# History:
-#   20211122:init this file.
-#   20211202:新增iic_bus,iic_addr配置参数.
+# ROS2 Humble参数文件。节点名必须与launch里的name保持一致,
+# 参数需要放在ros__parameters下面。
 ###################################################
-iic_bus: 1
-iic_addr: 0x50
-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_node:
+  ros__parameters:
+    iic_bus: 1
+    # 0x50的十进制值为80。也可以写成字符串"0x50"。
+    iic_addr: 80
+    topic_pub_hz: 100.0
+    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/iic_6dof_imu.launch

@@ -1,15 +0,0 @@
-<!---
-  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_file)" command="load"/>
-  </node>
-</launch>
-

iic_6dof_imu/launch/iic_6dof_imu.launch.py

@@ -0,0 +1,30 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # ROS2 launch通过参数文件启动IIC IMU节点,默认加载cfg/param.yaml。
+    default_config = PathJoinSubstitution([
+        FindPackageShare('iic_6dof_imu'),
+        'cfg',
+        'param.yaml',
+    ])
+
+    config_arg = DeclareLaunchArgument(
+        'iic_imu_cfg_file',
+        default_value=default_config,
+        description='iic_6dof_imu ROS2 parameter file',
+    )
+
+    imu_node = Node(
+        package='iic_6dof_imu',
+        executable='iic_6dof_imu_node.py',
+        name='iic_6dof_imu_node',
+        output='screen',
+        parameters=[LaunchConfiguration('iic_imu_cfg_file')],
+    )
+
+    return LaunchDescription([config_arg, imu_node])

iic_6dof_imu/package.xml

@@ -1,71 +1,24 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>iic_6dof_imu</name>
   <version>0.0.0</version>
-  <description>The iic_6dof_imu package</description>
+  <description>Read mini 6DOF IMU data from I2C and publish ROS2 topics/services.</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>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <!-- 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> -->
-
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
 
-  <!-- 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_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>python3-smbus</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
-  <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 -->
-
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

iic_6dof_imu/src/iic_6dof_imu_data.py

@@ -1,52 +1,85 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
 # Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
 # Author: corvin
 # Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
-#    这里注意self.RPY_CONST表示的是180.0/32768.0,self.ACC_CONST
-#    表示16.0/32768.0，self.ANG_CONST表示2000.0/32768.0.
 # History:
 #    20211122:Initial this file.
-#    20211202:init函数增加iic_bus,iic_addr参数，同时优化get_YPRAG函数.
-import smbus,rospy
-import numpy as np
+#    20260429:迁移到ROS2 Humble,去掉rospy依赖并使用Python原生int16转换。
+import smbus
+
+
+def _to_int16(low_byte, high_byte):
+    """将IMU寄存器的低字节在前格式转换为有符号16位整数。"""
+    value = (high_byte << 8) | low_byte
+    if value >= 0x8000:
+        value -= 0x10000
+    return value
+
 
 class MyIMU(object):
-    def __init__(self, iic_bus, iic_addr):
-        self.i2c  = smbus.SMBus(iic_bus)
+    def __init__(self, iic_bus, iic_addr, logger=None):
+        self.i2c = smbus.SMBus(iic_bus)
         self.addr = iic_addr
+        self.logger = logger
         self.RPY_CONST = 0.00549316
-        self.ACC_CONST = 0.00048828 
+        self.ACC_CONST = 0.00048828
         self.ANG_CONST = 0.06103516
 
+        self.raw_roll = 0.0
+        self.raw_pitch = 0.0
+        self.raw_yaw = 0.0
+        self.raw_ax = 0.0
+        self.raw_ay = 0.0
+        self.raw_az = 0.0
+        self.raw_gx = 0.0
+        self.raw_gy = 0.0
+        self.raw_gz = 0.0
+        self.raw_q0 = 1.0
+        self.raw_q1 = 0.0
+        self.raw_q2 = 0.0
+        self.raw_q3 = 0.0
+
+    def _log_error(self, msg):
+        if self.logger is not None:
+            self.logger.error(msg)
+        else:
+            print(msg)
+
     def get_YPRAG(self):
         try:
-            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
+            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])*self.RPY_CONST)
-            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])*self.RPY_CONST)
-            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])*self.RPY_CONST)
+            self._log_error("Read IMU RPYAG data error !")
+            return False
+
+        # RPY输出单位为度,加速度单位为g,角速度单位为度/秒。
+        self.raw_roll = float(_to_int16(rpy_data[0], rpy_data[1]) * self.RPY_CONST)
+        self.raw_pitch = float(_to_int16(rpy_data[2], rpy_data[3]) * self.RPY_CONST)
+        self.raw_yaw = float(_to_int16(rpy_data[4], rpy_data[5]) * self.RPY_CONST)
 
-            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])*self.ACC_CONST)
-            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])*self.ACC_CONST)
-            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])*self.ACC_CONST)
+        self.raw_ax = float(_to_int16(axyz_data[0], axyz_data[1]) * self.ACC_CONST)
+        self.raw_ay = float(_to_int16(axyz_data[2], axyz_data[3]) * self.ACC_CONST)
+        self.raw_az = float(_to_int16(axyz_data[4], axyz_data[5]) * self.ACC_CONST)
 
-            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])*self.ANG_CONST)
-            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])*self.ANG_CONST)
-            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])*self.ANG_CONST)
+        self.raw_gx = float(_to_int16(gxyz_data[0], gxyz_data[1]) * self.ANG_CONST)
+        self.raw_gy = float(_to_int16(gxyz_data[2], gxyz_data[3]) * self.ANG_CONST)
+        self.raw_gz = float(_to_int16(gxyz_data[4], gxyz_data[5]) * self.ANG_CONST)
+        return True
 
     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)
+            self._log_error("Read IMU quaternion data error !")
+            return False
+
+        # 四元数寄存器顺序为q0,q1,q2,q3,其中q0对应ROS消息中的w。
+        self.raw_q0 = float(_to_int16(quat_data[0], quat_data[1]) / 32768.0)
+        self.raw_q1 = float(_to_int16(quat_data[2], quat_data[3]) / 32768.0)
+        self.raw_q2 = float(_to_int16(quat_data[4], quat_data[5]) / 32768.0)
+        self.raw_q3 = float(_to_int16(quat_data[6], quat_data[7]) / 32768.0)
+        return True

iic_6dof_imu/src/iic_6dof_imu_node.py

@@ -1,127 +1,134 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
 # Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数，
-#    然后组装成ROS中的IMU消息格式，发布到/imu_data话题中，这样有需要的
-#    节点可以直接订阅该话题即可获取到imu当前的数据.
+# Description: 从IIC接口读取6DOF IMU模块数据,并发布ROS2话题和服务。
 # Author: corvin
 # History:
 #    20211122:Initial this file.
-#    20211203:增加imu_iic_bus,imu_iic_addr配置参数.
-import rospy
+#    20260429:迁移到ROS2 Humble.
 import math
 
-from std_msgs.msg import Float32
+import rclpy
+from rclpy.node import Node
 from sensor_msgs.msg import Imu
+from std_msgs.msg import Float32
+
+from iic_6dof_imu.srv import GetYawData
 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
-imu_iic_bus     = rospy.get_param('~iic_bus', 1)
-imu_iic_addr    = rospy.get_param('~iic_addr', '0x50')
-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, 0, 0,
-0, 0, 0,
-0, 0, 0
-]
-
-#Angular velocity covariance estimation
-imuMsg.angular_velocity_covariance = [
-0, 0, 0,
-0, 0, 0,
-0, 0, 0
-]
-
-#linear acceleration covariance estimation
-imuMsg.linear_acceleration_covariance = [
-0, 0, 0,
-0, 0, 0,
-0, 0, 0
-]
-
-seq = 0
-accel_factor = 9.806  #sensor accel g convert to m/s^2.
-
-myIMU = MyIMU(imu_iic_bus, int(imu_iic_addr))
-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)
-
-    #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()
+
+
+class Iic6DofImuNode(Node):
+    DEGREE_TO_RAD = math.pi / 180.0
+    ACC_FACTOR = 9.806
+
+    def __init__(self):
+        super().__init__('iic_6dof_imu_node')
+        self._load_parameters()
+
+        self.imu = MyIMU(self.iic_bus, self.iic_addr, self.get_logger())
+        self.yaw = 0.0
+
+        self.data_pub = self.create_publisher(Imu, self.data_topic_name, 10)
+        self.yaw_pub = self.create_publisher(Float32, self.yaw_topic_name, 10)
+        self.pitch_pub = self.create_publisher(Float32, self.pitch_topic_name, 10)
+        self.roll_pub = self.create_publisher(Float32, self.roll_topic_name, 10)
+        self.yaw_srv = self.create_service(GetYawData, self.get_yaw_srv, self._return_yaw_data)
+
+        timer_period = 1.0 / self.topic_pub_hz
+        self.timer = self.create_timer(timer_period, self._read_and_publish)
+        self.get_logger().info('Now 6DOF IMU Module is working ...')
+
+    def _load_parameters(self):
+        # ROS2参数需要先声明再读取,这些默认值可被cfg/param.yaml覆盖。
+        self.declare_parameter('iic_bus', 1)
+        self.declare_parameter('iic_addr', 0x50)
+        self.declare_parameter('topic_pub_hz', 100.0)
+        self.declare_parameter('pub_data_topic', 'imu_data')
+        self.declare_parameter('yaw_topic', 'yaw_data')
+        self.declare_parameter('pitch_topic', 'pitch_data')
+        self.declare_parameter('roll_topic', 'roll_data')
+        self.declare_parameter('link_name', 'base_imu_link')
+        self.declare_parameter('get_yaw_service', '/iic_imu_service/getYawData_service')
+
+        self.iic_bus = int(self.get_parameter('iic_bus').value)
+        self.iic_addr = self._parse_iic_addr(self.get_parameter('iic_addr').value)
+        self.topic_pub_hz = float(self.get_parameter('topic_pub_hz').value)
+        if self.topic_pub_hz <= 0.0:
+            self.get_logger().warning('topic_pub_hz <= 0, force set to 100Hz')
+            self.topic_pub_hz = 100.0
+
+        self.data_topic_name = self.get_parameter('pub_data_topic').value
+        self.yaw_topic_name = self.get_parameter('yaw_topic').value
+        self.pitch_topic_name = self.get_parameter('pitch_topic').value
+        self.roll_topic_name = self.get_parameter('roll_topic').value
+        self.link_name = self.get_parameter('link_name').value
+        self.get_yaw_srv = self.get_parameter('get_yaw_service').value
+
+    def _parse_iic_addr(self, value):
+        # 参数文件中可以写十进制80,也可以写字符串"0x50"。
+        if isinstance(value, str):
+            return int(value, 16)
+        return int(value)
+
+    def _return_yaw_data(self, request, response):
+        response.yaw = float(self.yaw)
+        return response
+
+    def _read_and_publish(self):
+        if not self.imu.get_YPRAG():
+            return
+
+        yaw_deg = float(self.imu.raw_yaw)
+        if yaw_deg >= 180.0:
+            yaw_deg -= 360.0
+
+        self.yaw = yaw_deg * self.DEGREE_TO_RAD
+        pitch = float(self.imu.raw_pitch) * self.DEGREE_TO_RAD
+        roll = float(self.imu.raw_roll) * self.DEGREE_TO_RAD
+
+        self.yaw_pub.publish(Float32(data=float(self.yaw)))
+        self.pitch_pub.publish(Float32(data=float(pitch)))
+        self.roll_pub.publish(Float32(data=float(roll)))
+
+        imu_msg = Imu()
+        imu_msg.header.stamp = self.get_clock().now().to_msg()
+        imu_msg.header.frame_id = self.link_name
+
+        # 加速度单位从g转换到m/s^2。这里沿用原ROS1代码的ENU方向约定,三轴取反。
+        imu_msg.linear_acceleration.x = -float(self.imu.raw_ax) * self.ACC_FACTOR
+        imu_msg.linear_acceleration.y = -float(self.imu.raw_ay) * self.ACC_FACTOR
+        imu_msg.linear_acceleration.z = -float(self.imu.raw_az) * self.ACC_FACTOR
+
+        # 角速度单位从度/秒转换到rad/s。
+        imu_msg.angular_velocity.x = float(self.imu.raw_gx) * self.DEGREE_TO_RAD
+        imu_msg.angular_velocity.y = float(self.imu.raw_gy) * self.DEGREE_TO_RAD
+        imu_msg.angular_velocity.z = float(self.imu.raw_gz) * self.DEGREE_TO_RAD
+
+        if not self.imu.get_quatern():
+            return
+
+        imu_msg.orientation.x = self.imu.raw_q1
+        imu_msg.orientation.y = self.imu.raw_q2
+        imu_msg.orientation.z = self.imu.raw_q3
+        imu_msg.orientation.w = self.imu.raw_q0
+
+        # 当前驱动未做噪声标定,协方差矩阵保持0。
+        imu_msg.orientation_covariance = [0.0] * 9
+        imu_msg.angular_velocity_covariance = [0.0] * 9
+        imu_msg.linear_acceleration_covariance = [0.0] * 9
+        self.data_pub.publish(imu_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = Iic6DofImuNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

imu_tools/.gitignore

@@ -1,15 +0,0 @@
-.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

@@ -1,15 +0,0 @@
-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

@@ -1,18 +0,0 @@
-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

@@ -1,31 +0,0 @@
-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

@@ -1,100 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,59 +0,0 @@
-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

@@ -1,180 +0,0 @@
-/*
-  @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

@@ -1,108 +0,0 @@
-/*
-  @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

@@ -1,34 +0,0 @@
-<!-- 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

@@ -1,25 +0,0 @@
-<?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

@@ -1,551 +0,0 @@
-/*
-  @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

@@ -1,42 +0,0 @@
-/*
-  @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

@@ -1,305 +0,0 @@
-/*
-  @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

@@ -1,217 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,74 +0,0 @@
-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

@@ -1,18 +0,0 @@
-#! /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

@@ -1,9 +0,0 @@
-<!-- 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

@@ -1,107 +0,0 @@
-/*
- *  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

@@ -1,41 +0,0 @@
-/*
- *  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

@@ -1,116 +0,0 @@
-/*
- *  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

@@ -1,48 +0,0 @@
-/*
- *  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

@@ -1,8 +0,0 @@
-#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

@@ -1,17 +0,0 @@
-<!-- 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

@@ -1,44 +0,0 @@
-<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

@@ -1,338 +0,0 @@
-/*
- *  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

@@ -1,35 +0,0 @@
-/*
- *  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

@@ -1,39 +0,0 @@
-/*
- *  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

@@ -1,393 +0,0 @@
-/*
- *  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

@@ -1,172 +0,0 @@
-/*
- *  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

@@ -1,121 +0,0 @@
-#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

@@ -1,117 +0,0 @@
-#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

@@ -1,102 +0,0 @@
-
-#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

@@ -1,81 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

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

imu_tools/imu_tools/package.xml

@@ -1,21 +0,0 @@
-<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

@@ -1,100 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,66 +0,0 @@
-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

@@ -1,28 +0,0 @@
-<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

@@ -1,13 +0,0 @@
-<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

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

imu_tools/rviz_imu_plugin/src/imu_acc_visual.cpp

@@ -1,173 +0,0 @@
-/*
- * 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

@@ -1,110 +0,0 @@
-/*
- * 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

@@ -1,144 +0,0 @@
-/*
- * 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

@@ -1,98 +0,0 @@
-/*
- * 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

@@ -1,331 +0,0 @@
-/*
- * 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

@@ -1,171 +0,0 @@
-/*
- * 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

@@ -1,179 +0,0 @@
-/*
- * 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

@@ -1,107 +0,0 @@
-/*
- * 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

@@ -1,202 +1,10 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(rviz_display_6dof_imu)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+find_package(ament_cmake REQUIRED)
 
-## 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}
+install(DIRECTORY cfg launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-## 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)
+ament_package()

rviz_display_6dof_imu/cfg/6dof_imu_display.rviz

@@ -1,5 +1,5 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
     Help Height: 123
     Name: Displays
     Property Tree Widget:
@@ -7,21 +7,21 @@ Panels:
         - /Global Options1
       Splitter Ratio: 0.6222222447395325
     Tree Height: 488
-  - Class: rviz/Selection
+  - Class: rviz_common/Selection
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
       - /2D Pose Estimate1
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
     Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
+  - Class: rviz_common/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-  - Class: rviz/Time
+  - Class: rviz_common/Time
     Experimental: false
     Name: Time
     SyncMode: 0
@@ -35,7 +35,7 @@ Visualization Manager:
   Displays:
     - Alpha: 0.699999988079071
       Cell Size: 0.5
-      Class: rviz/Grid
+      Class: rviz_default_plugins/Grid
       Color: 50; 115; 54
       Enabled: true
       Line Style:
@@ -83,26 +83,26 @@ Visualization Manager:
     Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+    - Class: rviz_default_plugins/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
+    - Class: rviz_default_plugins/SetGoal
+      Topic: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
       Single click: true
       Topic: /clicked_point
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
+      Class: rviz_default_plugins/Orbit
       Distance: 0.6284882426261902
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549

rviz_display_6dof_imu/launch/rviz_display_6dof_imu.launch

@@ -1,12 +0,0 @@
-<!---
-  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/launch/rviz_display_6dof_imu.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # 启动RViz2并加载IMU显示配置文件。
+    rviz_config = PathJoinSubstitution([
+        FindPackageShare('rviz_display_6dof_imu'),
+        'cfg',
+        '6dof_imu_display.rviz',
+    ])
+
+    return LaunchDescription([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='imu_rviz_node',
+            output='screen',
+            arguments=['-d', rviz_config],
+        )
+    ])

rviz_display_6dof_imu/package.xml

@@ -1,59 +1,19 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>rviz_display_6dof_imu</name>
   <version>0.0.0</version>
-  <description>The rviz_display_6dof_imu package</description>
+  <description>RViz2 launch and config for mini 6DOF IMU visualization.</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 -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
   <license>TODO</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rviz_imu_plugin</exec_depend>
 
-  <!-- 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 -->
-
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

serial_6dof_imu/CMakeLists.txt

@@ -1,193 +1,45 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(serial_6dof_imu)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
-## 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
-# )
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-## Generate services in the 'srv' folder
-add_service_files(
-  FILES
-  getYawData.srv
-  setYawZero.srv
-  setIICAddr.srv
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetYawData.srv"
+  "srv/SetIICAddr.srv"
+  "srv/SetYawZero.srv"
 )
 
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
+add_executable(serial_6dof_imu_node
+  src/serial_imu_node.cpp
+  src/proc_serial_data.cpp
+)
+target_include_directories(serial_6dof_imu_node PRIVATE include)
+target_compile_features(serial_6dof_imu_node PUBLIC cxx_std_17)
+ament_target_dependencies(serial_6dof_imu_node
+  rclcpp
   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
+# 同一个包内的C++节点使用本包生成的srv类型时,需要显式链接rosidl typesupport.
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(serial_6dof_imu_node "${cpp_typesupport_target}")
 
-## 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
+install(TARGETS serial_6dof_imu_node
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
+install(DIRECTORY cfg launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-## 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)
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

serial_6dof_imu/cfg/param.yaml

@@ -1,34 +1,20 @@
 ###########################################################
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用串口与IMU模块进行通信时的配置信息,根据需要修改
-# 可以配置的各参数如下(这里需要注意参数名称和参数之间要有空格隔开):
-#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
-#     模块进行通信,都是写ttyUSB0,1,2.
-#  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归零.
-#  get_yaw_data_srv:可以获取到yaw当前角度的服务.
-# Author: corvin
-# History:
-#   20211122:init this file.
+# ROS2 Humble参数文件。节点名必须与launch里的name保持一致,
+# 参数需要放在ros__parameters下面,否则ROS2不会加载。
 ###########################################################
-imu_dev: /dev/mini_imu
-pub_topic_hz: 100
+serial_imu_node:
+  ros__parameters:
+    imu_dev: /dev/mini_imu
+    pub_topic_hz: 100.0
 
-imu_link_name: base_imu_link
-pub_data_topic: imu_data
-temp_pub_topic: imu_temp
-yaw_pub_topic: yaw_data
-pitch_pub_topic: pitch_data
-roll_pub_topic: roll_data
-yaw_zero_topic: yaw_zero_topic
+    imu_link_name: base_imu_link
+    pub_data_topic: imu_data
+    temp_pub_topic: imu_temp
+    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
-get_yaw_data_srv: /serial_imu_service/getYawData_service
-set_iic_addr_srv: /serial_imu_service/setIICAddr_service
+    yaw_zero_service: /serial_imu_service/setYawZero_service
+    get_yaw_data_srv: /serial_imu_service/getYawData_service
+    set_iic_addr_srv: /serial_imu_service/setIICAddr_service

serial_6dof_imu/include/imu_data.h

@@ -8,6 +8,8 @@
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
 
+#include <string>
+
 int initSerialPort(const char* path);
 
 int getImuData(void);
@@ -17,7 +19,7 @@ float getAcc(int flag);
 float getAngular(int flag);
 float getAngle(int flag);
 float getQuat(int flag);
-float getTemp();
+float getTemp(void);
 
 int makeYawZero(void);
 int updateIICAddr(std::string input);

serial_6dof_imu/launch/serial_6dof_imu.launch

@@ -1,13 +0,0 @@
-<!--
-  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/launch/serial_6dof_imu.launch.py

@@ -0,0 +1,30 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # ROS2 launch通过参数文件启动串口IMU节点,默认加载cfg/param.yaml。
+    default_config = PathJoinSubstitution([
+        FindPackageShare('serial_6dof_imu'),
+        'cfg',
+        'param.yaml',
+    ])
+
+    config_arg = DeclareLaunchArgument(
+        'serial_6dof_imu_cfg',
+        default_value=default_config,
+        description='serial_6dof_imu ROS2 parameter file',
+    )
+
+    imu_node = Node(
+        package='serial_6dof_imu',
+        executable='serial_6dof_imu_node',
+        name='serial_imu_node',
+        output='screen',
+        parameters=[LaunchConfiguration('serial_6dof_imu_cfg')],
+    )
+
+    return LaunchDescription([config_arg, imu_node])

serial_6dof_imu/package.xml

@@ -1,72 +1,23 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>serial_6dof_imu</name>
   <version>0.0.0</version>
-  <description>The serial_6dof_imu package</description>
+  <description>Read mini 6DOF IMU data from serial port and publish ROS2 topics/services.</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>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <!-- 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> -->
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
 
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
-  <!-- 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 -->
-
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

serial_6dof_imu/src/proc_serial_data.cpp

@@ -4,32 +4,53 @@
  * Author: corvin
  * History:
  *   20211122:init this file.
+ *   20260429:迁移到ROS2 Humble,并优化串口读取解析逻辑以跟随100Hz输出.
 ******************************************************************/
-#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      55            //每次从串口中读取的总字节数
+#include <imu_data.h>
+
+#include <cerrno>
+#include <cstdio>
+#include <cstring>
+#include <fcntl.h>
+#include <sstream>
+#include <string>
+#include <termios.h>
+#include <unistd.h>
+
+#define  READ_BUF_SIZE 256 //每次从串口批量读取的缓冲区大小,可容纳多组IMU数据
+#define  FRAME_LEN     11  //IMU串口协议中每个数据帧固定为11字节
 #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],temp;
+#define  FRAME_HEADER  0x55
+#define  FRAME_ACCE    0x51
+#define  FRAME_GYRO    0x52
+#define  FRAME_ANGLE   0x53
+#define  FRAME_QUAT    0x59
+
+#define  FRAME_MASK_ACCE  0x01
+#define  FRAME_MASK_GYRO  0x02
+#define  FRAME_MASK_ANGLE 0x04
+#define  FRAME_MASK_QUAT  0x08
+#define  FRAME_MASK_ALL   (FRAME_MASK_ACCE | FRAME_MASK_GYRO | FRAME_MASK_ANGLE | FRAME_MASK_QUAT)
+
+static unsigned char r_buf[READ_BUF_SIZE];  //一次从串口中读取的数据存储缓冲区
+static int port_fd = -1;                    //串口打开时的文件描述符
+static float acce[3], angular[3], angle[3], quater[4], temp;
 
 /**************************************
  * Description:关闭串口文件描述符.
  *************************************/
 int closeSerialPort(void)
 {
+    if (port_fd < 0)
+    {
+        return 0;
+    }
+
     int ret = close(port_fd);
+    port_fd = -1;
     return ret;
 }
 
@@ -38,15 +59,15 @@ int closeSerialPort(void)
  ****************************************************************/
 static int send_unlockCmd(int fd)
 {
-    int ret = 0;
     unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
-    ret = write(fd, unLockCmd, sizeof(unLockCmd));
-    if(ret != sizeof(unLockCmd))
+    int ret = write(fd, unLockCmd, sizeof(unLockCmd));
+    if(ret != (int)sizeof(unLockCmd))
     {
-        ROS_ERROR("Send IMU module unlock cmd error !");
+        std::fprintf(stderr, "Send IMU module unlock cmd error !\n");
         return -1;
     }
-    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
+
+    usleep(10 * 1000); //延时10ms后再发送配置命令,满足IMU模块协议时序要求
     return 0;
 }
 
@@ -55,308 +76,329 @@ static int send_unlockCmd(int fd)
  *****************************************************************/
 static int send_saveCmd(int fd)
 {
-    int ret = 0;
-    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
-    ret = write(fd, saveCmd, sizeof(saveCmd));
-    if(ret != sizeof(saveCmd))
+    unsigned char saveCmd[5] = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+    int ret = write(fd, saveCmd, sizeof(saveCmd));
+    if(ret != (int)sizeof(saveCmd))
     {
-        ROS_ERROR("Send IMU module save cmd error !");
+        std::fprintf(stderr, "Send IMU module save cmd error !\n");
         return -1;
     }
-    ros::Duration(0.1).sleep(); //delay 100ms才能发送其他配置命令
+
+    usleep(100 * 1000); //保存配置需要更长等待时间,否则下一条命令可能被模块忽略
     return 0;
 }
 
 /*****************************************************************
- * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
- *   发送控制命令,最后就是需要发送两遍解锁命令、保存命令的操作.
+ * Description:向IMU模块的串口中发送控制命令.
  *****************************************************************/
 static int send_data(int fd, unsigned char *send_buffer, int length)
 {
-    int ret = 0;
+    if (fd < 0)
+    {
+        std::fprintf(stderr, "IMU serial port is not opened !\n");
+        return -1;
+    }
 
-    #if 0 //打印发送给IMU模块的数据,用于调试
-    printf("Send %d byte to IMU:", length);
-    for(int i=0; i<length; i++)
+    if (send_unlockCmd(fd) != 0)
     {
-        printf("0x%02x ", send_buffer[i]);
+        return -1;
     }
-    printf("\n");
-    #endif
-    send_unlockCmd(fd); //发送解锁命令
 
-    //发送完控制命令,需要延时100ms
-    ret = write(fd, send_buffer, length*sizeof(unsigned char));
+    int ret = write(fd, send_buffer, length * sizeof(unsigned char));
     if(ret != length)
     {
-        ROS_ERROR("Send IMU module control cmd error !");
+        std::fprintf(stderr, "Send IMU module control cmd error !\n");
         return -2;
     }
-    ros::Duration(0.1).sleep(); //delay 100ms才能发送其他配置命令
 
-    send_saveCmd(fd);  //发送保存命令
+    usleep(100 * 1000); //控制命令发送后等待模块处理完成
+    send_saveCmd(fd);   //发送保存命令,使配置在模块中生效
     return 0;
 }
 
 /**************************************************************
- * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
- *   解析读取其中的44个字节,正好是4帧数据,每一帧数据都是11个字节.
- *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ * Description:将低字节在前、高字节在后的协议数据转换为有符号16位数.
+ *************************************************************/
+static short bytes_to_short(unsigned char low, unsigned char high)
+{
+    return (short)(((unsigned short)high << 8) | low);
+}
+
+/**************************************************************
+ * Description:根据串口数据协议解析单个字节。返回true表示已收齐一组
+ *   完整IMU数据(加速度、角速度、欧拉角、四元数各一帧).
  *************************************************************/
-static void parse_serialData(unsigned char chr)
+static bool parse_serialData(unsigned char chr)
 {
-    static unsigned char chrBuf[BYTE_CNT];
-    static unsigned char chrCnt = 0;  //记录读取的第几个字节
+    static unsigned char chrBuf[FRAME_LEN];
+    static unsigned char chrCnt = 0;    //记录当前帧已经缓存的字节数
+    static unsigned char frameMask = 0; //记录当前一组IMU数据中已解析到的帧类型
 
-    signed short sData[4]; //save 8 Byte有效信息
-    unsigned char i = 0;   //用于for循环
-    unsigned char frameSum = 0;  //存储数据帧的校验和
+    unsigned char frameSum = 0;
 
-    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
+    //还没同步到帧头时直接丢弃杂散字节,减少无效memmove和校验开销.
+    if ((chrCnt == 0) && (chr != FRAME_HEADER))
+    {
+        return false;
+    }
 
-    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
-    if(chrCnt < 11)
-        return;
+    chrBuf[chrCnt++] = chr;
+    if(chrCnt < FRAME_LEN)
+    {
+        return false;
+    }
 
-    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
-    for(i=0; i<10; i++)
+    for(unsigned char i = 0; i < FRAME_LEN - 1; i++)
     {
         frameSum += chrBuf[i];
     }
 
-    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
-    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
-    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
+    if ((chrBuf[0] != FRAME_HEADER) || (frameSum != chrBuf[FRAME_LEN - 1]))
     {
-        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
-        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
-        return;
+        int nextHeader = -1;
+        for (unsigned char i = 1; i < FRAME_LEN; i++)
+        {
+            if (chrBuf[i] == FRAME_HEADER)
+            {
+                nextHeader = i;
+                break;
+            }
+        }
+
+        //校验失败时尽量保留后续可能的帧头,避免因为丢字节导致长时间不同步.
+        if (nextHeader > 0)
+        {
+            chrCnt = FRAME_LEN - nextHeader;
+            memmove(&chrBuf[0], &chrBuf[nextHeader], chrCnt);
+        }
+        else
+        {
+            chrCnt = 0;
+        }
+        return false;
     }
 
-    #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])  //根据数据帧不同类型来进行解析数据
+    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;
+        case FRAME_ACCE: //x,y,z轴加速度输出,单位为g
+            acce[0] = bytes_to_short(chrBuf[2], chrBuf[3]) * ACCE_CONST;
+            acce[1] = bytes_to_short(chrBuf[4], chrBuf[5]) * ACCE_CONST;
+            acce[2] = bytes_to_short(chrBuf[6], chrBuf[7]) * ACCE_CONST;
+            frameMask |= FRAME_MASK_ACCE;
             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;
-            temp       = ((float)(((short)chrBuf[9]<<8)|chrBuf[8]))/100.0;
+        case FRAME_GYRO: //x,y,z轴角速度输出,单位为度/秒
+            angular[0] = bytes_to_short(chrBuf[2], chrBuf[3]) * ANGULAR_CONST;
+            angular[1] = bytes_to_short(chrBuf[4], chrBuf[5]) * ANGULAR_CONST;
+            angular[2] = bytes_to_short(chrBuf[6], chrBuf[7]) * ANGULAR_CONST;
+            temp       = bytes_to_short(chrBuf[8], chrBuf[9]) / 100.0;
+            frameMask |= FRAME_MASK_GYRO;
             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;
+        case FRAME_ANGLE: //欧拉角输出,依次为roll,pitch,yaw,单位为度
+            angle[0] = bytes_to_short(chrBuf[2], chrBuf[3]) * ANGLE_CONST;
+            angle[1] = bytes_to_short(chrBuf[4], chrBuf[5]) * ANGLE_CONST;
+            angle[2] = bytes_to_short(chrBuf[6], chrBuf[7]) * ANGLE_CONST;
+            frameMask |= FRAME_MASK_ANGLE;
             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;
+        case FRAME_QUAT: //四元数输出,顺序为q0,q1,q2,q3
+            quater[0] = bytes_to_short(chrBuf[2], chrBuf[3]) / 32768.0;
+            quater[1] = bytes_to_short(chrBuf[4], chrBuf[5]) / 32768.0;
+            quater[2] = bytes_to_short(chrBuf[6], chrBuf[7]) / 32768.0;
+            quater[3] = bytes_to_short(chrBuf[8], chrBuf[9]) / 32768.0;
+            frameMask |= FRAME_MASK_QUAT;
             break;
         default:
-            ROS_ERROR("parse imu data frame type error !");
+            //模块可能额外输出磁力计或状态帧,当前ROS消息不使用,直接忽略可避免高频打印拖慢100Hz读取.
             break;
     }
     chrCnt = 0;
+
+    if ((frameMask & FRAME_MASK_ALL) == FRAME_MASK_ALL)
+    {
+        frameMask = 0;
+        return true;
+    }
+    return false;
 }
 
 /********************************************************************
- * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
- *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
+ * Description:将yaw角度归零.
  *******************************************************************/
 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) //通过串口发送命令失败
+    int ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
+    if(ret != 0)
     {
-        ROS_ERROR("Send yaw zero control cmd error !");
+        std::fprintf(stderr, "Send yaw zero control cmd error !\n");
         return -1;
     }
-    ROS_WARN("Set yaw to zero radian successfully !");
 
+    std::fprintf(stderr, "Set yaw to zero radian successfully !\n");
     return 0;
 }
 
 /*******************************************************************
- * Description:更新IMU模块的IIC地址,这里通过发送串口命令方式来更新IIC地址,
- *   完整的控制命令为:0xFF 0xAA 0x1A 0x** 0x00,更新的IIC地址为第4个字节.
+ * Description:更新IMU模块的IIC地址.
  ******************************************************************/
 int updateIICAddr(std::string input)
 {
-    int ret = 0;
     std::stringstream num;
     int addr = 0;
-    const char *iicAddr = NULL;
     unsigned char updateIICAddrCmd[5] = {0xFF, 0xAA, 0x1A, 0x00, 0x00};
-    
-    iicAddr = input.c_str();
-    num<<std::hex<<iicAddr+2<<std::endl;
-    num>>addr;
-    updateIICAddrCmd[3] = addr;
-
-    #if 0
-    for (int i=0; i<5; i++)
+
+    if (input.rfind("0x", 0) == 0 || input.rfind("0X", 0) == 0)
     {
-        printf("0x%02x ", updateIICAddrCmd[i]);
+        num << std::hex << input.substr(2);
     }
-    printf("\n\n");
-    #endif
- 
-    ret = send_data(port_fd, updateIICAddrCmd, sizeof(updateIICAddrCmd));
+    else
+    {
+        num << std::hex << input;
+    }
+    num >> addr;
+
+    if (addr < 0x00 || addr > 0x7F)
+    {
+        std::fprintf(stderr, "IIC address out of range: %s\n", input.c_str());
+        return -1;
+    }
+
+    updateIICAddrCmd[3] = (unsigned char)addr;
+    int ret = send_data(port_fd, updateIICAddrCmd, sizeof(updateIICAddrCmd));
     if(ret != 0)
     {
-        ROS_ERROR("Update IMU Module IIC Address Error !");
+        std::fprintf(stderr, "Update IMU Module IIC Address Error !\n");
         return -1;
     }
-    ROS_WARN("Update IMU Module IIC Address [%s] successfully !", input.c_str());
 
+    std::fprintf(stderr, "Update IMU Module IIC Address [%s] successfully !\n", input.c_str());
     return 0;
 }
 
 /*****************************************************************
- * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
- *   输入参数的意义如下:
- *   const char* path:IMU设备的挂载地址;
+ * Description:根据串口配置信息连接IMU串口.
  ****************************************************************/
 int initSerialPort(const char* path)
 {
     struct termios terminfo;
-    bzero(&terminfo, sizeof(terminfo));
 
-    port_fd = open(path, O_RDWR|O_NOCTTY);
-    if (-1 == port_fd)
+    port_fd = open(path, O_RDWR | O_NOCTTY);
+    if (port_fd == -1)
     {
-        ROS_ERROR("Open 6DOF IMU dev error:%s", path);
+        std::fprintf(stderr, "Open 6DOF IMU dev error: %s\n", path);
         return -1;
     }
 
-    //判断当前连接的设备是否为终端设备
-    if (isatty(STDIN_FILENO) == 0)
+    if (isatty(port_fd) == 0)
     {
-        ROS_ERROR("IMU dev isatty error !");
+        std::fprintf(stderr, "IMU dev isatty error !\n");
+        closeSerialPort();
         return -2;
     }
 
-    /*设置串口通信波特率-115200bps*/
+    if (tcgetattr(port_fd, &terminfo) != 0)
+    {
+        std::fprintf(stderr, "Get imu serial port attr error !\n");
+        closeSerialPort();
+        return -3;
+    }
+
+    //raw模式会关闭行缓冲、回显、特殊字符转换等终端处理,保证二进制串口协议按字节原样读取.
+    cfmakeraw(&terminfo);
+
     cfsetispeed(&terminfo, B115200);
     cfsetospeed(&terminfo, B115200);
 
-    //设置数据位 - 8 bit
-    terminfo.c_cflag |= CLOCAL|CREAD;
+    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_iflag &= ~(IXON | IXOFF | IXANY);
+#ifdef CRTSCTS
+    terminfo.c_cflag &= ~CRTSCTS;
+#endif
 
-    //设置等待时间和最小接收字符
+    //VMIN=1表示至少读到1个字节就返回,上层会持续缓存并拼出完整帧.
     terminfo.c_cc[VTIME] = 0;
     terminfo.c_cc[VMIN]  = 1;
 
-    //清除串口缓存的数据
     tcflush(port_fd, TCIFLUSH);
-
-    if((tcsetattr(port_fd, TCSANOW, &terminfo)) != 0)
+    if(tcsetattr(port_fd, TCSANOW, &terminfo) != 0)
     {
-        ROS_ERROR("set imu serial port attr error !");
+        std::fprintf(stderr, "Set imu serial port attr error !\n");
+        closeSerialPort();
         return -3;
     }
 
     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:返回IMU芯片内部实时温度值.
- ********************************************/
 float getTemp(void)
 {
     return temp;
 }
 
 /*************************************************************
- * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取88个字节,然后需要从这些字节中进行
- *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
- *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ * Description:从串口批量读取并持续解析。read()一次可能读到半组、
+ *  一组或多组IMU数据,这里会保留未消费完的数据。只有解析到
+ *  加速度、角速度、欧拉角、四元数各一帧后才返回给上层发布。
  *************************************************************/
 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;
-    }
+    static int buf_pos = 0;
+    static int buf_len = 0;
 
-    //printf("recv data:%d byte\n", data_len); //一次性从串口中读取的总数据字节数
-    for (int i=0; i<data_len; i++) //将读取到的数据进行解析
+    while (true)
     {
-        //printf("0x%02x ", r_buf[i]);
-        parse_serialData(r_buf[i]);
+        //优先消费上一次read后尚未解析完的数据,避免同一批数据中的后续样本被丢掉.
+        while (buf_pos < buf_len)
+        {
+            if (parse_serialData(r_buf[buf_pos++]))
+            {
+                return 0;
+            }
+        }
+
+        buf_pos = 0;
+        buf_len = 0;
+
+        data_len = read(port_fd, r_buf, sizeof(r_buf));
+        if(data_len < 0)
+        {
+            if (errno == EINTR)
+            {
+                continue;
+            }
+            std::fprintf(stderr, "Read serial port data failed !\n");
+            closeSerialPort();
+            return -1;
+        }
+        if(data_len == 0)
+        {
+            continue;
+        }
+        buf_len = data_len;
     }
-    //printf("\n\n");
-
-    return 0;
 }

serial_6dof_imu/src/serial_imu_node.cpp

@@ -1,189 +1,257 @@
 /******************************************************************
  * Copyright:2016-2021 www.corvin.cn ROS小课堂
- * Description:使用串口来读取IMU的数据,并通过ROS话题topic将数据发布出来.
- *   芯片中默认读取出来的加速度数据单位是g,需要将其转换为ROS中加速度规定的
- *   m/s2才能发布.ROS中使用的坐标系为ENU东北天坐标系,x轴指向东,y轴指向北,
- *   z轴指向天空.
+ * Description:使用串口读取IMU数据,并通过ROS2话题和服务发布/控制.
  * Author: corvin
  * History:
  *   20211122:init this file.
- *   20220220:ROS中IMU的坐标系为ENU东北天坐标系,所以需要修改线加速度的
- *       数据正方向,与XYZ三轴正方向都相反,此时Z轴加速度G为正值.
+ *   20260429:迁移到ROS2 Humble.
 ******************************************************************/
-#include <ros/ros.h>
-#include <tf/tf.h>
-#include <sensor_msgs/Imu.h>
-#include <std_msgs/Float32.h>
-#include <std_msgs/Empty.h>
-#include <imu_data.h>
-#include "serial_6dof_imu/setYawZero.h"
-#include "serial_6dof_imu/getYawData.h"
-#include "serial_6dof_imu/setIICAddr.h"
+#include <cmath>
+#include <memory>
+#include <string>
 
-static float g_yawData;  //全局变量,存储当前yaw值,可以通过服务来得到该值
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/float32.hpp>
 
-/**********************************************************
- * Description:将yaw角度归零的话题回调函数,只要往话题中发布一条
- *   std_msgs::Empty类型消息,即可将yaw角度归零.
- *********************************************************/
-void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
-{
-    makeYawZero();
-}
+#include <imu_data.h>
+#include "serial_6dof_imu/srv/get_yaw_data.hpp"
+#include "serial_6dof_imu/srv/set_iic_addr.hpp"
+#include "serial_6dof_imu/srv/set_yaw_zero.hpp"
 
-/******************************************************************
- * Description:使用service方式来进行yaw角度归零,这里是服务的回调
- *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
- *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
- *   命令执行失败.
- *****************************************************************/
-bool yawZeroService(serial_6dof_imu::setYawZero::Request &req,
-                    serial_6dof_imu::setYawZero::Response &res)
+class SerialImuNode : public rclcpp::Node
 {
-    res.status = makeYawZero();
-    return true;
-}
+public:
+    SerialImuNode()
+    : Node("serial_imu_node")
+    {
+        load_parameters();
+        create_ros_interfaces();
 
-/**********************************************************************
- * Description:通过service服务调用方式来获取到yaw角度信息.
- **********************************************************************/
-bool getYawDataService(serial_6dof_imu::getYawData::Request &req,
-                       serial_6dof_imu::getYawData::Response &res)
-{
-    res.yaw = g_yawData;
-    return true;
-}
+        int ret = initSerialPort(imu_dev_.c_str());
+        if(ret < 0)
+        {
+            RCLCPP_ERROR(get_logger(), "init 6DOF IMU module serial port error: %s", imu_dev_.c_str());
+            serial_ok_ = false;
+            return;
+        }
 
-/*******************************************************************
- * Description:通过service方式来更新IIC地址,默认地址为0x50,可以更新的地址
- *   范围为0x00 - 0x7F,注意更新的地址不能与IIC总线上其他设备地址冲突,这样才
- *   能在IIC总线上读取到所有地址上IIC设备的数据.
- ******************************************************************/
-bool setIICAddrService(serial_6dof_imu::setIICAddr::Request &req,
-                       serial_6dof_imu::setIICAddr::Response &res)
-{
-    res.status = updateIICAddr(req.address);
-    return true;
-}
+        imu_msg_.header.frame_id = imu_link_name_;
+        //三个协方差矩阵暂时置0,表示当前驱动没有对噪声方差做标定估计.
+        imu_msg_.orientation_covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+        imu_msg_.angular_velocity_covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+        imu_msg_.linear_acceleration_covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+        serial_ok_ = true;
+        RCLCPP_INFO(get_logger(), "Now 6DOF IMU module is working...");
+    }
+
+    ~SerialImuNode() override
+    {
+        closeSerialPort();
+    }
+
+    bool serial_ok() const
+    {
+        return serial_ok_;
+    }
+
+    double publish_hz() const
+    {
+        return pub_topic_hz_;
+    }
+
+    bool read_and_publish_once()
+    {
+        if (!serial_ok_)
+        {
+            return false;
+        }
+
+        if(getImuData() < 0)
+        {
+            RCLCPP_ERROR(get_logger(), "read imu data failed, stop serial imu node");
+            return false;
+        }
+
+        const double roll  = getAngle(0) * DEGREE_TO_RAD;
+        const double pitch = getAngle(1) * DEGREE_TO_RAD;
+        double yaw = getAngle(2) * DEGREE_TO_RAD;
+        if(yaw >= M_PI)
+        {
+            yaw -= 2.0 * M_PI;
+        }
+
+        //保存当前yaw值,服务回调会直接返回这个最新值.
+        yaw_data_ = static_cast<float>(yaw);
+
+        std_msgs::msg::Float32 yaw_msg;
+        std_msgs::msg::Float32 pitch_msg;
+        std_msgs::msg::Float32 roll_msg;
+        std_msgs::msg::Float32 temp_msg;
+        yaw_msg.data   = yaw_data_;
+        pitch_msg.data = static_cast<float>(pitch);
+        roll_msg.data  = static_cast<float>(roll);
+        temp_msg.data  = getTemp();
+
+        yaw_pub_->publish(yaw_msg);
+        pitch_pub_->publish(pitch_msg);
+        roll_pub_->publish(roll_msg);
+        temp_pub_->publish(temp_msg);
+
+        imu_msg_.header.stamp = now();
+        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模块输出角速度单位为度/秒,ROS2 sensor_msgs/Imu要求单位为rad/s.
+        imu_msg_.angular_velocity.x = getAngular(0) * DEGREE_TO_RAD;
+        imu_msg_.angular_velocity.y = getAngular(1) * DEGREE_TO_RAD;
+        imu_msg_.angular_velocity.z = getAngular(2) * DEGREE_TO_RAD;
+
+        //IMU模块输出加速度单位为g,ROS2 sensor_msgs/Imu要求单位为m/s^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_pub_->publish(imu_msg_);
+        return true;
+    }
+
+private:
+    static constexpr double DEGREE_TO_RAD = M_PI / 180.0;
+    static constexpr double ACC_FACTOR = 9.806;
+
+    void load_parameters()
+    {
+        //ROS2中参数需要先声明再读取,这里给出默认值,也可以被launch加载的yaml覆盖.
+        imu_dev_ = declare_parameter<std::string>("imu_dev", "/dev/mini_imu");
+        imu_link_name_ = declare_parameter<std::string>("imu_link_name", "base_imu_link");
+        pub_topic_hz_ = declare_parameter<double>("pub_topic_hz", 100.0);
+        imu_topic_name_ = declare_parameter<std::string>("pub_data_topic", "imu_data");
+        yaw_zero_topic_ = declare_parameter<std::string>("yaw_zero_topic", "yaw_zero_topic");
+        yaw_zero_service_ = declare_parameter<std::string>("yaw_zero_service", "/serial_imu_service/setYawZero_service");
+        yaw_data_service_ = declare_parameter<std::string>("get_yaw_data_srv", "/serial_imu_service/getYawData_service");
+        set_iic_addr_service_ = declare_parameter<std::string>("set_iic_addr_srv", "/serial_imu_service/setIICAddr_service");
+        yaw_pub_topic_ = declare_parameter<std::string>("yaw_pub_topic", "yaw_data");
+        pitch_pub_topic_ = declare_parameter<std::string>("pitch_pub_topic", "pitch_data");
+        roll_pub_topic_ = declare_parameter<std::string>("roll_pub_topic", "roll_data");
+        temp_pub_topic_ = declare_parameter<std::string>("temp_pub_topic", "imu_temp");
+
+        if (pub_topic_hz_ <= 0.0)
+        {
+            RCLCPP_WARN(get_logger(), "pub_topic_hz <= 0, force set to 100Hz");
+            pub_topic_hz_ = 100.0;
+        }
+    }
+
+    void create_ros_interfaces()
+    {
+        auto qos = rclcpp::SensorDataQoS();
+        imu_pub_ = create_publisher<sensor_msgs::msg::Imu>(imu_topic_name_, qos);
+        yaw_pub_ = create_publisher<std_msgs::msg::Float32>(yaw_pub_topic_, 10);
+        pitch_pub_ = create_publisher<std_msgs::msg::Float32>(pitch_pub_topic_, 10);
+        roll_pub_ = create_publisher<std_msgs::msg::Float32>(roll_pub_topic_, 10);
+        temp_pub_ = create_publisher<std_msgs::msg::Float32>(temp_pub_topic_, 10);
+
+        yaw_zero_sub_ = create_subscription<std_msgs::msg::Empty>(
+            yaw_zero_topic_,
+            10,
+            [this](std_msgs::msg::Empty::SharedPtr)
+            {
+                int ret = makeYawZero();
+                RCLCPP_INFO(get_logger(), "yaw zero topic command finished, status=%d", ret);
+            });
+
+        yaw_zero_srv_ = create_service<serial_6dof_imu::srv::SetYawZero>(
+            yaw_zero_service_,
+            [this](
+                const std::shared_ptr<serial_6dof_imu::srv::SetYawZero::Request>,
+                std::shared_ptr<serial_6dof_imu::srv::SetYawZero::Response> response)
+            {
+                response->status = makeYawZero();
+                RCLCPP_INFO(get_logger(), "yaw zero service finished, status=%d", response->status);
+            });
+
+        get_yaw_srv_ = create_service<serial_6dof_imu::srv::GetYawData>(
+            yaw_data_service_,
+            [this](
+                const std::shared_ptr<serial_6dof_imu::srv::GetYawData::Request>,
+                std::shared_ptr<serial_6dof_imu::srv::GetYawData::Response> response)
+            {
+                response->yaw = yaw_data_;
+            });
+
+        set_iic_srv_ = create_service<serial_6dof_imu::srv::SetIICAddr>(
+            set_iic_addr_service_,
+            [this](
+                const std::shared_ptr<serial_6dof_imu::srv::SetIICAddr::Request> request,
+                std::shared_ptr<serial_6dof_imu::srv::SetIICAddr::Response> response)
+            {
+                response->status = updateIICAddr(request->address);
+                RCLCPP_INFO(
+                    get_logger(), "set iic addr service finished, address=%s, status=%d",
+                    request->address.c_str(), response->status);
+            });
+    }
+
+    bool serial_ok_{false};
+    float yaw_data_{0.0F};
+    double pub_topic_hz_{100.0};
+
+    std::string imu_dev_;
+    std::string imu_link_name_;
+    std::string imu_topic_name_;
+    std::string yaw_zero_topic_;
+    std::string yaw_zero_service_;
+    std::string yaw_data_service_;
+    std::string set_iic_addr_service_;
+    std::string yaw_pub_topic_;
+    std::string pitch_pub_topic_;
+    std::string roll_pub_topic_;
+    std::string temp_pub_topic_;
+
+    sensor_msgs::msg::Imu imu_msg_;
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr yaw_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pitch_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr roll_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr temp_pub_;
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr yaw_zero_sub_;
+    rclcpp::Service<serial_6dof_imu::srv::SetYawZero>::SharedPtr yaw_zero_srv_;
+    rclcpp::Service<serial_6dof_imu::srv::GetYawData>::SharedPtr get_yaw_srv_;
+    rclcpp::Service<serial_6dof_imu::srv::SetIICAddr>::SharedPtr set_iic_srv_;
+};
 
-/********************************************************************
- * Description:入口主函数,负责通过topic和service来发布处理数据.
- *******************************************************************/
 int main(int argc, char **argv)
 {
-    float yaw, pitch, roll;
-    std::string imu_dev;
-    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 temp_pub_topic;
-    std::string yaw_zero_topic;
-    std::string yaw_zero_service;
-    std::string yaw_data_service;
-    std::string set_iic_addr_service;
-
-    int pub_topic_hz = 0;  //话题发布imu数据的频率
-    const float degree2Rad = 0.017453292;  //3.1415926/180.0,将角度转换为弧度的系数
-    const float acc_factor = 9.806; //重力加速度常量
-
-    ros::init(argc, argv, "imu_data_pub_node");
-    ros::NodeHandle handle;
-
-    //launch文件中加载yaml配置文件，然后从yaml配置文件中读取各参数
-    ros::param::get("~imu_dev",          imu_dev);
-    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("~temp_pub_topic",   temp_pub_topic);
-    ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
-    ros::param::get("~roll_pub_topic",   roll_pub_topic);
-    ros::param::get("~get_yaw_data_srv", yaw_data_service);
-    ros::param::get("~set_iic_addr_srv", set_iic_addr_service);
-
-    //初始化imu模块串口,根据设备号与IMU建立连接
-    int ret = initSerialPort(imu_dev.c_str());
-    if(ret < 0) //通过串口连接IMU模块失败
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<SerialImuNode>();
+
+    if (!node->serial_ok())
     {
-        ROS_ERROR("init 6DOF IMU module serial port error !");
-        closeSerialPort();
+        rclcpp::shutdown();
         return -1;
     }
-    ROS_INFO("Now 6DOF IMU module is working...");
-
-    //定义服务,分别是yaw角度归零和获取yaw当前角度,更新IIC地址
-    ros::ServiceServer setyawSrv  = handle.advertiseService(yaw_zero_service,  yawZeroService);
-    ros::ServiceServer getYawSrv  = handle.advertiseService(yaw_data_service,  getYawDataService);
-    ros::ServiceServer setIICSrv  = handle.advertiseService(set_iic_addr_service, setIICAddrService);
-
-    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::Publisher temp_pub  = handle.advertise<std_msgs::Float32>(temp_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;
-    std_msgs::Float32 temp_msg;
-    while(ros::ok())
+
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node);
+    rclcpp::Rate loop_rate(node->publish_hz());
+
+    while(rclcpp::ok())
     {
-        if(getImuData() < 0)
+        if (!node->read_and_publish_once())
+        {
             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;
-        temp_msg.data  = getTemp();
-        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
-        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
-        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
-        temp_pub.publish(temp_msg);   //将temp值通过话题发布出去
-
-        //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, 0, 0,
-                                          0, 0, 0,
-                                          0, 0, 0};
-        //三轴角速度
-        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, 0, 0,
-                                               0, 0, 0,
-                                               0, 0, 0};
-        //三轴线加速度,这里规定xyz三轴线加速度坐标系为ENU
-        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, 0, 0,
-                                                  0, 0, 0,
-                                                  0, 0, 0};
-
-        imu_pub.publish(imu_msg); //将imu数据通过话题发布出去
-        ros::spinOnce();
+        //读取发布之后处理一次订阅和服务回调,避免阻塞100Hz主数据流.
+        executor.spin_some();
         loop_rate.sleep();
     }
 
-    closeSerialPort(); //关闭与IMU模块的串口连接
+    rclcpp::shutdown();
     return 0;
 }