minibot初始化程序

.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

.gitignore

@@ -0,0 +1,5 @@
+build/
+devel/
+*.pyc
+*.so
+.vscode/

README.md

@@ -1,2 +1,17 @@
 # ros_minibot_ws
 
+## 1. 更新、编译代码
+在源码根目录下可以发现有一个bash脚本文件update_code.sh,该脚本文件为了省去每次在终端中输入命令来更新、编译代码的繁琐步骤。这里只需要执行该脚本文件就可以自动的更新、编译代码，执行脚本的方法如下，在终端中执行：   
+`./update_code.sh`
+
+## 2. 启动运行代码
+这里只需要执行如下命令就可以将整个小车启动起来，进入到基本工作状态，命令如下：   
+`roslaunch robot_bringup robot_bringup`
+
+## 3. 启动键盘遥控小车移动
+这里使用了teleop_twist_keyboard来遥控小车四处移动，启动命令如下：   
+`rosrun control_robot control_robot.py`
+
+## 4. 查看摄像头画面
+现在可以直接查看摄像头的实时画面，但是在树莓派上查看的话画面会有一点卡顿，建议还是在本地电脑上查看画面。若在树莓派上查看摄像头画面的话，在终端中执行命令如下：   
+`rqt_image_view`

src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/melodic/share/catkin/cmake/toplevel.cmake

src/control_robot/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(control_robot)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+   control_robot.py
+   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+

src/control_robot/control_robot.py

@@ -0,0 +1,186 @@
+#!/usr/bin/env python
+# coding:utf-8
+
+# Description: 通过键盘遥控小车行进，摄像头舵机转动,2DOF机械臂转动.
+# History:
+#    20200726:增加控制左右转动舵机的代码.
+#    20210913:优化代码.
+#    20211117:将控制舵机转动的代码进行整合优化,精简代码.
+#    20220224:更新软件包名,代码文件名.
+
+from __future__ import print_function
+import rospy, roslib
+import sys,select,termios,tty
+from geometry_msgs.msg import Twist
+from ros_arduino_msgs.srv import ServoWrite
+
+msg = """
+使用键盘控制小车的夹爪和移动,控制按键如下:
+------------------------------
+     (左前) (前进) (右前)
+       u      i      o
+(左转) j      k      l (右转)
+       m      <      >
+     (左后) (后退) (右后)
+------------------------------
+q/z : 每次按照10%同比例来增加或减小线速度和角速度
+w/x : 每次按照10%比例来增加或减小线速度
+e/c : 每次按照10%比例来增加或减小角速度
+1/2 : 相机向上/向下转动
+3/4 : 相机向左/向右转动
+5/6 : 夹爪向上/向下转动
+7/8 : 夹爪打开/关闭操作
+-------------------------------
+Ctrl+c: 退出键盘控制模式
+"""
+
+moveBindings = {
+        'i':(1,0),
+        'j':(0,1),
+        'l':(0,-1),
+        ',':(-1,0),
+        'u':(1,1),
+        'o':(1,-1),
+        'm':(-1,-1),
+        '.':(-1,1),
+    }
+speedBindings={
+        'q':(1.1,1.1),
+        'z':(0.9,0.9),
+        'w':(1.1,1),
+        'x':(0.9,1),
+        'e':(1,1.1),
+        'c':(1,0.9),
+    }
+cameraUpDownBindings={
+        '1':(-5),
+        '2':(5),
+    }
+cameraLeftRightBindings={
+        '3':(5),
+        '4':(-5),
+    }
+gripperUpDownBindings={
+        '5':(5),
+        '6':(-5),
+    }
+gripperOpenCloseBindings={
+        '7':(-5),
+        '8':(5),
+    }
+
+#等待键盘按键被按下,获取到按键后将其返回
+def getKey():
+    tty.setraw(sys.stdin.fileno())
+    select.select([sys.stdin], [], [], 0)
+    key = sys.stdin.read(1)
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    return key
+
+def vels(speed, turn):
+    return "当前控制速度: 线速度: %s m/s 转弯角速度: %s rad/s" %(speed,turn)
+
+def angles(angle):
+    return "当前角度:  %s " % (angle)
+
+#调用service: mobilebase_arduino/servo_write控制对应舵机转动
+def client_srv(ser_id, ser_angle):
+    rospy.wait_for_service("mobilebase_arduino/servo_write")
+    try:
+        camera_client = rospy.ServiceProxy("mobilebase_arduino/servo_write", ServoWrite)
+        resp = camera_client.call(ser_id, ser_angle)
+    except (rospy.ServiceException, rospy.ROSException) as e:
+        rospy.logwarn("Service call failed: %s"%e)
+
+if __name__=="__main__":
+    settings = termios.tcgetattr(sys.stdin)
+
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
+    rospy.init_node('control_robot_node')
+
+    #初始化控制命令参数,线速度0.2m/s，角速度1.0rad/s
+    speed = 0.2
+    turn = 1.0
+
+    x  = 0
+    th = 0
+    status = 0
+    cameraUpDownAngle     = 90
+    cameraLeftRightAngle  = 90
+    gripperUpDownAngle    = 90
+    gripperOpenCloseAngle = 75
+
+    try:
+        print(msg)
+        print(vels(speed, turn))
+
+        while True:
+            x = 0
+            th = 0
+            key = getKey()
+            status = (status + 1) % 20
+            if (status == 0):
+                print(msg)
+
+            if key in moveBindings.keys():
+                x = moveBindings[key][0]
+                th = moveBindings[key][1]
+            elif key in speedBindings.keys():
+                speed = speed * speedBindings[key][0]
+                turn = turn * speedBindings[key][1]
+                print(vels(speed,turn))
+            elif key in cameraLeftRightBindings.keys():
+                print(angles(cameraLeftRightAngle))
+                if (cameraLeftRightAngle <= 180 and cameraLeftRightAngle >=0):
+                    if(cameraLeftRightAngle == 180 and cameraLeftRightBindings[key] > 0) or\
+                      (cameraLeftRightAngle == 0 and cameraLeftRightBindings[key] < 0):
+                      continue
+                    else:
+                        cameraLeftRightAngle = cameraLeftRightAngle + cameraLeftRightBindings[key]
+                        client_srv(0, cameraLeftRightAngle)
+            elif key in cameraUpDownBindings.keys():
+                print(angles(cameraUpDownAngle))
+                if (cameraUpDownAngle <= 180 and cameraUpDownAngle >= 60):
+                        if (cameraUpDownAngle == 180 and cameraUpDownBindings[key] > 0) or\
+                           (cameraUpDownAngle == 60 and cameraUpDownBindings[key] < 0):
+                            continue
+                        else:
+                            cameraUpDownAngle = cameraUpDownAngle + cameraUpDownBindings[key]
+                            client_srv(1, cameraUpDownAngle)
+            elif key in gripperUpDownBindings.keys():
+                print(angles(gripperUpDownAngle))
+                if (gripperUpDownAngle <= 130 and gripperUpDownAngle >= 65):
+                    if(gripperUpDownAngle == 130 and gripperUpDownBindings[key] > 0) or \
+                        (gripperUpDownAngle == 65 and gripperUpDownBindings[key] < 0):
+                        continue
+                    else:
+                        gripperUpDownAngle = gripperUpDownAngle + gripperUpDownBindings[key]
+                        client_srv(2, gripperUpDownAngle)
+            elif key in gripperOpenCloseBindings.keys():
+                print(angles(gripperOpenCloseAngle))
+                if (gripperOpenCloseAngle <= 120 and gripperOpenCloseAngle >= 75):
+                    if(gripperOpenCloseAngle == 120 and gripperOpenCloseBindings[key] > 0) or\
+                      (gripperOpenCloseAngle == 75 and gripperOpenCloseBindings[key] < 0):
+                        continue
+                    else:
+                        gripperOpenCloseAngle = gripperOpenCloseAngle + gripperOpenCloseBindings[key]
+                        client_srv(3, gripperOpenCloseAngle)
+            else:
+                if (key == '\x03'):
+                    break
+
+            twist = Twist()
+            twist.linear.x = x*speed; twist.linear.y = 0; twist.linear.z = 0;
+            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
+            pub.publish(twist)
+
+    except Exception as e:
+        print(e)
+    finally:
+        twist = Twist()
+        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
+        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
+        pub.publish(twist)
+
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+

src/control_robot/package.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<package>
+  <name>control_robot</name>
+  <version>0.6.2</version>
+  <description>Generic keyboard teleop for twist robots.</description>
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>rospy</run_depend>
+</package>

src/infrared_maze/CMakeLists.txt

@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(infrared_maze)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  ros_arduino_msgs
+  roscpp
+  geometry_msgs
+  nav_msgs
+  tf
+  std_msgs
+)
+
+## 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
+#   ros_arduino_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 infrared_maze
+#  CATKIN_DEPENDS ros_arduino_msgs roscpp
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/infrared_maze.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/infrared_maze.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
+# install(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_infrared_maze.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)

src/infrared_maze/README.md

@@ -0,0 +1,19 @@
+# blackTornadoRobot红外走迷宫功能
+走迷宫功能包，可通过红外传感器数据使小车在迷宫中行进。
+
+在param.yaml文件中可调整一些走迷宫时用到的参数。
+
+小车走迷宫的主要逻辑为：将迷宫看作由50cmX50cm方格添加障碍墙之后组成的环境，小车每次行进皆为50cm即行走一格，转弯角度为90度。小车每行进一格后会获取当前红外传感器数据，优先左转，其次直行，最后右转。
+
+### 建图运行方法
+首先启动小车
+
+```sh
+$ roslaunch robot_bringup robot_bringup.launch
+```
+
+然后运行走迷宫程序
+```sh
+$ roslaunch infrared_maze infrared_maze.launch
+```
+

src/infrared_maze/cfg/param.yaml

@@ -0,0 +1,32 @@
+#####################################################
+# Description:使用小车上三个红外测距传感器进行避障
+#   走迷宫,这里是一些可以配置的参数.
+#   infrared_topic:订阅的红外传感器发布数据的话题.
+#   cmd_topic:控制小车移动的话题名.
+#   yaw_service:发布IMU模块yaw偏行角的服务.
+#   infrared_service:发布红外测据信息的服务名.
+#   linear_x:移动时前进速度大小.
+#   angular_speed:原地转圈时角速度大小.
+#   warn_dist:小车是否可以前进的距离.
+#   tolerance_dist:距离容忍值.
+#   odom_frame:odom的tf名称.
+#   base_frame:底盘的tf名称.
+#   forward_dist:每次前进时移动的距离.
+# Author: corvin
+# History:
+#   20200404:init this file.
+#   20200406:增加yaw service,rate参数.
+#   20200412:增加获取红外测距信息的service.
+#   20200424:更新注释,增加参数介绍.
+######################################################
+infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
+cmd_topic: /cmd_vel
+yaw_service: /imu_node/GetYawData
+infrared_service: /mobilebase_arduino/getInfraredDistance
+odom_frame: /odom_combined
+base_frame: /base_footprint
+linear_x: 0.27
+angular_speed: 1.0
+warn_dist: 0.25
+tolerance_dist: 0.05
+forward_dist: 0.50

src/infrared_maze/launch/infrared_maze.launch

@@ -0,0 +1,15 @@
+<!--
+  Author: jally, corvin
+  Description:
+    maze with infrared
+  History:
+    20200325: intial this file.
+-->
+<launch>
+  <arg name="cfg_file" default="$(find infrared_maze)/cfg/param.yaml" />
+
+  <!-- startup infrared_obstacle_avoidance_node -->
+  <node pkg="infrared_maze" type="infrared_maze_node" name="infrared_maze_node" output="screen">
+    <rosparam file="$(arg cfg_file)" command="load" />
+  </node>
+</launch>

src/infrared_maze/package.xml

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

src/infrared_maze/src/infrared_maze.cpp

@@ -0,0 +1,377 @@
+/***************************************************************
+ Description:三轮小车进行红外避障走迷宫.根据前,左,右三个红外测距
+   信息来进行移动,这里的移动策略是每次移动0.5m,然后再判断距离.
+   移动时向左转优先,其次直行,最后是右转.
+ Author: jally, corvin
+ History:
+    20200404:增加可以直接转90度,掉头的功能 by corvin.
+    20200412:增加使用服务方式来获取红外测距值-corvin.
+    20200424:增加每次前进时移动距离的参数,可以配置.-corvin
+ **************************************************************/
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_listener.h>
+#include "rasp_imu_hat_6dof/GetYawData.h"
+#include "ros_arduino_msgs/InfraredSensors.h"
+#include "ros_arduino_msgs/GetInfraredDistance.h"
+
+
+#define  GO_FORWARD   0
+#define  TURN_LEFT    1
+#define  TURN_RIGHT   2
+#define  TURN_BACK    3
+#define  HORIZ_MOVE   4
+
+#define  ERROR       -1
+
+//global variable
+ros::Publisher twist_pub;
+ros::ServiceClient yawSrvClient;
+rasp_imu_hat_6dof::GetYawData yawSrv;
+
+//红外测距相关的服务
+ros::ServiceClient distSrvClient;
+ros_arduino_msgs::GetInfraredDistance distSrv;
+
+float warn_dist = 0.25;  //warn check distance
+float tolerance_dist = 0.05; //对警告距离的容忍值
+float forward_dist = 0.50; //每次前进移动时走的距离
+
+float front_dist;
+float left_dist;
+float right_dist;
+
+std::string odomFrame;
+std::string baseFrame;
+
+float linear_x_speed = 0.27;
+float angular_speed = 1.0;
+
+float start_yaw_data = 0.0;
+int turnFlag = ERROR;
+
+/*********************************************************
+ * Descripition: 发布控制移动的消息,这里对于平面移动的
+ *   机器人只需要控制linear_x和angular_z即可.
+ ********************************************************/
+void publishTwistCmd(float linear_x, float angular_z)
+{
+    geometry_msgs::Twist twist_msg;
+
+    twist_msg.linear.x = linear_x;
+    twist_msg.linear.y = 0.0;
+    twist_msg.linear.z = 0.0;
+
+    twist_msg.angular.x = 0.0;
+    twist_msg.angular.y = 0.0;
+    twist_msg.angular.z = angular_z;
+
+    twist_pub.publish(twist_msg);
+}
+
+/**********************************************************
+ * Description:调用IMU板发布出来的yaw信息,可以根据该数据
+ *   来修正转弯时不准确的问题.注意这里的yaw数据为弧度值.
+ *   注意往右旋转为负值,左旋转为正值.右手定则.
+ *   注意在ROS中IMU数据中yaw的范围,具体表示如下:
+ *                0  -0
+ *                -----
+ *              /      \
+ *         1.57|        | -1.57
+ *              \      /
+ *               ------
+ *             3.14 -3.14
+ **********************************************************/
+void yawUpdate(const float start_yaw)
+{
+    float target = 0.0;
+    float diff = 10.0;
+    float angular = 0.0;
+    float now_yaw = 0.0;
+
+    //开始计算修正后的目标角度
+    if((turnFlag == GO_FORWARD)||(turnFlag == HORIZ_MOVE))
+    {
+        target = start_yaw;
+    }
+    else if(turnFlag == TURN_LEFT)
+    {
+        target = start_yaw + M_PI/2.0;
+        if(target > M_PI)
+        {
+            target = target - M_PI*2.0;
+        }
+    }
+    else if(turnFlag == TURN_RIGHT)
+    {
+        target = start_yaw - M_PI/2.0;
+        if(target < -M_PI)
+        {
+            target = target + M_PI*2.0;
+        }
+    }
+    else //turn back
+    {
+        target = start_yaw + M_PI;
+        if(target > M_PI)
+        {
+            target = target - M_PI*2.0;
+        }
+    }
+
+    //开始不断的计算偏差,调整角度
+    while(fabs(diff) > 0.01)
+    {
+        yawSrvClient.call(yawSrv);
+        now_yaw = yawSrv.response.yaw;
+        ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
+
+        diff = target - now_yaw;
+        if(diff > 0)
+        {
+            angular = angular_speed;
+        }
+        else
+        {
+            angular = -angular_speed;
+        }
+        ROS_WARN("Yaw diff:%f, angular:%f", diff, angular);
+        publishTwistCmd(0, angular);
+        ros::Duration(0.04).sleep();
+    }
+    publishTwistCmd(0, 0); //在修正角度结束后应该立刻停止小车转动
+    ROS_INFO("Correct Yaw diff over !");
+}
+
+/****************************************************
+ * Description:控制小车前进移动0.5m
+ ***************************************************/
+void goForward(const float check_dist)
+{
+    tf::TransformListener tf_Listener;
+    tf::StampedTransform tf_Transform;
+    float dist = 0.0; //每次前进移动的距离
+    float front_dist = 0.0;
+
+    try
+    {
+        tf_Listener.waitForTransform(odomFrame, baseFrame, ros::Time(0), ros::Duration(5.0));
+    }
+    catch(tf::TransformException &ex)
+    {
+        ROS_ERROR("tf_Listener.waitForTransform timeout error !");
+        ROS_ERROR("%s", ex.what());
+        ros::shutdown();
+    }
+
+    tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+    float x_start = tf_Transform.getOrigin().x();
+    float y_start = tf_Transform.getOrigin().y();
+
+    distSrvClient.call(distSrv); //通过服务调用来获取测距值
+    front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
+    ROS_INFO("Front dist:%f", front_dist);
+    while((dist<check_dist)&&(front_dist>tolerance_dist*4.0)&&(ros::ok()))
+    {
+        ROS_INFO("Go forward, dist:%f", dist);
+        publishTwistCmd(linear_x_speed, 0);
+        ros::Duration(0.10).sleep(); //100 ms
+
+        tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0),tf_Transform);
+        float x = tf_Transform.getOrigin().x();
+        float y = tf_Transform.getOrigin().y();
+        dist = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
+
+        distSrvClient.call(distSrv); //通过服务调用来获取测距值
+        front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
+        ROS_INFO("Front dist:%f", front_dist);
+    }
+    publishTwistCmd(0, 0); //stop move
+    ROS_INFO("Go forward finish !!!");
+}
+
+/*************************************************************
+ * Description:控制小车左右转90度,或者掉头180度.这里实现小车
+ *   转弯功能是通过不断监听odom和base_link之间的tf转换完成的.
+ ************************************************************/
+int controlTurn(int flag)
+{
+    tf::TransformListener tf_Listener;
+    tf::StampedTransform tf_Transform;
+    float angular = 0.0;
+    float check_angle = 0.0;
+
+    switch(flag)
+    {
+        case TURN_LEFT: //向左转动90°
+            turnFlag = TURN_LEFT;
+            angular = angular_speed;
+            check_angle = M_PI/2.0;
+            break;
+
+        case TURN_RIGHT: //向右转动90°
+            turnFlag = TURN_RIGHT;
+            angular = -angular_speed;
+            check_angle = M_PI/2.0;
+            break;
+
+        case TURN_BACK: //小车原地掉头180度
+            turnFlag = TURN_BACK;
+            check_angle = M_PI;
+            angular = angular_speed;
+            break;
+
+        case GO_FORWARD: //小车直行50cm
+            goForward(forward_dist);
+            return 1;
+
+        default:
+            ROS_ERROR("Turn flag error !");
+            return -1;
+    }
+
+    try
+    {
+        tf_Listener.waitForTransform(odomFrame, baseFrame, ros::Time(0), ros::Duration(5.0));
+    }
+    catch(tf::TransformException &ex)
+    {
+        ROS_ERROR("tf_Listener.waitForTransform timeout error !");
+        ROS_ERROR("%s", ex.what());
+        ros::shutdown();
+
+        return -2;
+    }
+
+    //ROS_INFO("Check_angle:%f, angular_speed:%f",check_angle, angular);
+    tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+    float last_angle = fabs(tf::getYaw(tf_Transform.getRotation()));
+    float turn_angle = 0.0;
+    ROS_INFO("first_angle:%f", last_angle);
+    while((fabs(turn_angle) < check_angle)&&(ros::ok()))
+    {
+        publishTwistCmd(0, angular); //原地转弯,不需要linear_x,y的控制
+        ros::Duration(0.10).sleep(); //100 ms
+
+        try
+        {
+            tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+        }
+        catch(tf::TransformException &ex)
+        {
+            ROS_ERROR("%s", ex.what());
+            continue;
+        }
+        float rotation = fabs(tf::getYaw(tf_Transform.getRotation()));
+        float delta_angle = fabs(rotation - last_angle);
+
+        turn_angle += delta_angle;
+        last_angle = rotation;
+        ROS_INFO("Turn angle:%f", turn_angle);
+    }
+    publishTwistCmd(0, 0); //原地转弯完成后,需要立刻停止小车的旋转
+    ROS_INFO("Turning finish !!!");
+    ros::Duration(0.1).sleep(); //100 ms
+
+    return 0;
+}
+
+/***************************************************************************
+ * Description:检查三个传感器数据,然后控制小车移动.这里需要防止小车在原地
+ *   进行连续两次旋转,在每次转弯后必须有个直行.这里需要注意的是当小车在转弯
+ *   后一定要有一个可以修正转弯角度的功能,否则转弯后角度不准确小车前进方向
+ *   会错.这里的走迷宫策略是左转优先,中间是直行,最后是右转.
+ *   当前方,左边,右边都无法移动时,进行掉头动作,然后直行.
+ **************************************************************************/
+void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
+        float tolerance, float warn_distance)
+{
+    static int flag = 0; //记录是否转过弯的标志
+
+    yawSrvClient.call(yawSrv); //通过服务调用来获取yaw值
+    start_yaw_data = yawSrv.response.yaw; //记录当前yaw值,用于后面修正
+
+    //判断能否左转,并且上一次动作不能是转弯
+    if((infrared_l == (float)0.3)&&(flag != 1))
+    {
+        ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
+        turnFlag = TURN_LEFT;
+        flag = 1; //设置转弯标志
+    }
+    else if(infrared_f > warn_distance)//判断能否直行
+    {
+        //始终控制小车保持在过道的中间位置
+        ROS_WARN("go Forward ! yaw_data: %f", start_yaw_data);
+        turnFlag = GO_FORWARD;
+        flag = 0; //设置直行标志
+    }
+    else if((infrared_r == (float)0.3)&&(flag != 1))//判断能否右转
+    {
+        ROS_WARN("turn Right ! yaw_data: %f", start_yaw_data);
+        turnFlag = TURN_RIGHT;
+        flag = 1; //设置转弯标志
+    }
+    else //小车掉头
+    {
+        ROS_WARN("turn Back ! yaw_data: %f", start_yaw_data);
+        turnFlag = TURN_BACK;
+    }
+
+    controlTurn(turnFlag); //开始执行动作
+}
+
+/*****************************************************************************
+ * Description: 订阅红外测距传感器话题后的回调函数,根据三个传感器数据进行走
+ *   迷宫.
+ ****************************************************************************/
+void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
+{
+    ROS_INFO("front left right distance:[%f %f %f]", msg->front_dist,
+            msg->left_dist, msg->right_dist);
+    front_dist = msg->front_dist;
+    left_dist  = msg->left_dist;
+    right_dist = msg->right_dist;
+
+    //根据三个传感器反馈的测距值开始移动
+    checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist, warn_dist);
+    yawUpdate(start_yaw_data); //移动完成后开始更新yaw值
+}
+
+/************************************************************
+ * Description:主函数初始化节点,读取配置参数,订阅和发布话题,
+ *   在订阅的红外测距数据话题回调函数中进行移动,通过调用yaw
+ *   服务进行转弯角度的修正.
+ ************************************************************/
+int main(int argc, char **argv)
+{
+    std::string infrared_topic;
+    std::string cmd_topic;
+    std::string yaw_service;
+    std::string infrared_service;
+
+    ros::init(argc, argv, "infrared_maze_node");
+    ros::NodeHandle handle;
+
+    ros::param::get("~infrared_topic", infrared_topic);
+    ros::param::get("~cmd_topic", cmd_topic);
+    ros::param::get("~yaw_service", yaw_service);
+    ros::param::get("~infrared_service", infrared_service);
+    ros::param::get("~linear_x",  linear_x_speed);
+    ros::param::get("~angular_speed", angular_speed);
+    ros::param::get("~warn_dist", warn_dist);
+    ros::param::get("~tolerance_dist", tolerance_dist);
+    ros::param::get("~odom_frame", odomFrame);
+    ros::param::get("~base_frame", baseFrame);
+    ros::param::get("~forward_dist", forward_dist);
+
+    ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
+    twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
+    yawSrvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    distSrvClient = handle.serviceClient<ros_arduino_msgs::GetInfraredDistance>(infrared_service);
+
+    ros::spin();
+    return 0;
+}
+

src/rasp_camera/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rasp_camera)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES rasp_camera
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/rasp_camera.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/rasp_camera_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_rasp_camera.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)

src/rasp_camera/launch/rasp_camera.launch

@@ -0,0 +1,24 @@
+<launch>
+  <arg name="enable_raw" default="false"/>
+  <arg name="enable_imv" default="false"/>
+  <arg name="camera_id" default="0"/>
+  <arg name="camera_frame_id" default="raspicam"/>
+  <arg name="camera_name" default="camerav1_1280x720"/>
+
+  <node type="raspicam_node" pkg="raspicam_node" name="raspicam_node" output="screen">
+    <param name="private_topics" value="true"/>
+
+    <param name="camera_frame_id" value="$(arg camera_frame_id)"/>
+    <param name="enable_raw" value="$(arg enable_raw)"/>
+    <param name="enable_imv" value="$(arg enable_imv)"/>
+    <param name="camera_id" value="$(arg camera_id)"/>
+
+    <param name="camera_info_url" value="package://raspicam_node/camera_info/camerav1_1280x720.yaml"/>
+    <param name="camera_name" value="$(arg camera_name)"/>
+    <param name="width" value="320"/>
+    <param name="height" value="240"/>
+
+    <!-- We are running at 20fps to reduce motion blur -->
+    <param name="framerate" value="20"/>
+  </node>
+</launch>

src/rasp_camera/package.xml

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

src/rasp_imu_hat_6dof/.gitignore

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

src/rasp_imu_hat_6dof/README.md

@@ -0,0 +1,51 @@
+# rasp_imu_hat_6dof
+
+为树莓派IMU扩展板创建的ROS代码仓库，直接将该软件包源码放在ROS工作区src目录下即可。然后使用catkin_make就可以进行编译了。完整的下载命令如下:  
+`git clone https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git`  
+
+----
+## 使用串口发布IMU数据
+当编译完成后可以使用如下命令启动发布imu数据:  
+`roslaunch serial_imu_hat_6dof serial_imu_hat.launch`  
+
+当启动上述命令后,可以在ROS中查看到如下话题和服务可以调用:  
+* 发布imu数据的话题名: /imu_data
+* 发布imu模块温度的话题名: /imu_temp
+* 发布yaw角度的话题名: /yaw_data
+* 可将yaw角度归零的话题名: /yaw_zero_topic
+* 可将yaw角度归零的服务名: /yaw_zero_service
+* 可修改串口波特率的服务: /baud_update_service
+* 控制D0,D1,D2,D3输出数字高低电平的服务: /pin_outHL_service  
+* 可得到当前yaw角度的服务: /imu_node/GetYawData  
+对于以上这些话题名和服务名,都可以在serial_imu_hat_6dof/cfg/param.yaml配置文件中进行修改.  
+
+1. 如果想使用话题方式,将yaw角度归零,那就可以执行如下命令:  
+`rostopic pub -1 /yaw_zero_topic std_msgs/Empty "{}"`  
+
+2. 如果想使用服务调用方式,将yaw角度归零,那执行如下命令:  
+`rosservice call /yaw_zero_service "{}"`  
+
+3. 如果想修改串口通信的波特率,那就可以执行如下命令:  
+`rosservice call /baud_update_service "index: 1"`  
+这里需要注意index后面的数字,１表示更新波特率为9600,2表示为57600,3表示115200,
+当更新完波特率后,需要修改本地配置文件中串口波特率为新的,否则会无法正常获取imu数据.  
+
+4. 如果需要控制D0引脚输出高电平,那就可以使用如下命令:  
+rosservice call /pin_outHL_service "D0: 1  
+D1: 0  
+D2: 0  
+D3: 0"  
+这里可以看到D0后面的数字为1,那就表明D0引脚输出高电平.若想修改其他引脚的高低电平状态,那就修改D0,D1,D2,D3后面的对应数字即可.  
+
+5. 如果想通过服务调用方式来获取到当前的yaw角度信息,可在终端中执行如下命令:  
+`rosservice call /imu_node/GetYawData`  
+当然这些不仅可以在终端中执行命令获取到yaw数据,还可以在代码中编写服务的客户端程序,请求字段为空,就可以在response字段中获取到当前yaw角度.  
+
+----
+## 使用IIC发布IMU数据
+`roslaunch rasp_imu_hat_6dof imu_data_pub.launch`  
+
+----
+## 在Rviz中显示查看IMU效果
+`roslaunch rasp_imu_hat_6dof imu_rviz_display.launch`  
+

src/rasp_imu_hat_6dof/imu_tools/.gitignore

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

src/rasp_imu_hat_6dof/imu_tools/.travis.yml

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

src/rasp_imu_hat_6dof/imu_tools/Dockerfile-melodic

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

src/rasp_imu_hat_6dof/imu_tools/Dockerfile-noetic

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

src/rasp_imu_hat_6dof/imu_tools/LICENSE

@@ -0,0 +1,5 @@
+The imu_filter_madgwick package is licensed under the GNU General Public Licence v3 or later.
+See LICENSE.gplv3 for the text of the license.
+
+The rest of the code in this repository is licensed under the 3-clause BSD license.
+See LICENSE.bsd for the text of the license.

src/rasp_imu_hat_6dof/imu_tools/LICENSE.bsd

@@ -0,0 +1,31 @@
+Copyright (c) 2021, DFKI GmbH
+Copyright (c) 2015, City University of New York
+Copyright (c) 2012, Willow Garage, Inc.
+Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
+Copyright (c) 2010, CCNY Robotics Lab
+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 copyright holder 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.
+

src/rasp_imu_hat_6dof/imu_tools/LICENSE.gplv3

@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    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/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    <program>  Copyright (C) <year>  <name of author>
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<http://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.

src/rasp_imu_hat_6dof/imu_tools/README.md

@@ -0,0 +1,65 @@
+IMU tools for ROS
+===================================
+
+Overview
+-----------------------------------
+
+IMU-related filters and visualizers. The stack contains:
+
+ * `imu_filter_madgwick`: a filter which fuses angular velocities,
+accelerations, and (optionally) magnetic readings from a generic IMU 
+device into an orientation. Based on the work of [1].
+
+ * `imu_complementary_filter`: a filter which fuses angular velocities,
+accelerations, and (optionally) magnetic readings from a generic IMU 
+device into an orientation quaternion using a novel approach based on a complementary fusion. Based on the work of [2].
+
+ * `rviz_imu_plugin` a plugin for rviz which displays `sensor_msgs::Imu`
+messages
+
+Installing
+-----------------------------------
+
+### From source ###
+
+[Create a catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace)
+(e.g., `~/ros-hydro-ws/`) and source the `devel/setup.bash` file.
+
+Make sure you have git installed:
+
+    sudo apt-get install git-core
+
+Download the stack from our repository into your catkin workspace (e.g.,
+`ros-hydro-ws/src`; use the proper branch for your distro, e.g., `groovy`,
+`hydro`...):
+
+    git clone -b <distro> https://github.com/ccny-ros-pkg/imu_tools.git
+
+Install any dependencies using [rosdep](http://www.ros.org/wiki/rosdep).
+
+    rosdep install imu_tools
+
+Compile the stack:
+
+    cd ~/ros-hydro-ws
+    catkin_make
+
+More info
+-----------------------------------
+
+http://wiki.ros.org/imu_tools
+
+License
+-----------------------------------
+
+ * `imu_filter_madgwick`: currently licensed as GPL, following the original implementation
+ 
+ * `imu_complementary_filter`: BSD
+
+ * `rviz_imu_plugin`: BSD
+
+References
+-----------------------------------
+ [1] http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+
+ [2] http://www.mdpi.com/1424-8220/15/8/19302

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/launch/complementary_filter.launch

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/src/complementary_filter.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/src/complementary_filter_node.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/imu_filter_nodelet.xml

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_nodelet.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/launch/imu_filter_madgwick.launch

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/src/imu_filter.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/src/imu_filter_node.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/src/imu_filter_nodelet.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/src/imu_filter_ros.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/src/stateless_orientation.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/test/madgwick_test.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/test/stateless_orientation_test.cpp

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/test/test_helpers.h

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/plugin_description.xml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/rosdoc.yaml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_acc_visual.cpp

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_acc_visual.h

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_axes_visual.cpp

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_axes_visual.h

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_display.cpp

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_display.h

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_orientation_visual.h

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

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rasp_imu_hat_6dof)
+
+find_package(catkin REQUIRED COMPONENTS 
+    rospy
+    std_msgs
+    dynamic_reconfigure
+    message_generation)
+
+add_service_files(
+    FILES
+    GetYawData.srv
+)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+)
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+generate_dynamic_reconfigure_options(
+    cfg/imu.cfg
+)
+
+catkin_package(
+    CATKIN_DEPENDS
+    std_msgs
+    message_runtime
+)
+
+include_directories(
+    ${catkin_INCLUDE_DIRS}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY cfg
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY src
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS
+	nodes/imu_node.py
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
+)
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/LICENSE

@@ -0,0 +1,72 @@
+Apache License 
+Version 2.0, January 2004 
+http://www.apache.org/licenses/
+TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+1. Definitions.
+
+"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document.
+
+"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License.
+
+"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity.
+
+"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License.
+
+"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files.
+
+"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types.
+
+"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below).
+
+"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof.
+
+"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution."
+
+"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work.
+
+2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form.
+
+3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed.
+
+4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions:
+
+(a) You must give any other recipients of the Work or Derivative Works a copy of this License; and
+
+(b) You must cause any modified files to carry prominent notices stating that You changed the files; and
+
+(c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and
+
+(d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License.
+
+You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License.
+
+5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions.
+
+6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file.
+
+7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License.
+
+8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages.
+
+9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability.
+
+END OF TERMS AND CONDITIONS
+
+APPENDIX: How to apply the Apache License to your work.
+
+To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives.
+
+Copyright [yyyy] [name of copyright owner]
+
+Licensed under the Apache License, Version 2.0 (the "License"); 
+you may not use this file except in compliance with the License. 
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software 
+distributed under the License is distributed on an "AS IS" BASIS, 
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
+See the License for the specific language governing permissions and 
+limitations under the License.

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/README.md

@@ -0,0 +1,3 @@
+# rasp_imu_hat_6dof
+
+基于树莓派制作的扩展板，可以直接插在树莓派上使用，这里代码为ROS上使用的软件包源码，可以下载直接在catkin_ws/src目录下，然后编译使用

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/imu.cfg

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+PACKAGE = "rasp_imu_hat_6dof"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("yaw_calibration", double_t, 0, "Yaw Calibration", 0, -180, 180)
+
+exit(gen.generate(PACKAGE, "rasp_imu_hat_6dof", "imu"))

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/imu_display.rviz

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

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/param.yaml

@@ -0,0 +1,19 @@
+###################################################
+# Description:使用IIC总线来读取IMU模块的数据,并
+#   通过话题将imu数据发送出来.这里是可以配置的
+#   一些参数.各参数意义如下:
+#   pub_data_topic:发布imu数据的话题名.
+#   pub_temp_topic:发布当前imu模块温度的话题名.
+#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
+#   link_name:imu模块的urdf中link名字.
+#   pub_hz:上述各话题发布的频率.
+# Author: corvin
+# History:
+#   20200406:init this file.
+#   20200410:更新默认发布imu话题数据频率为20hz.
+###################################################
+pub_data_topic: imu_data
+pub_temp_topic: imu_temp
+yaw_topic: yaw_data
+link_name: base_imu_link
+pub_hz: 20

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/launch/imu_data_pub.launch

@@ -0,0 +1,16 @@
+<!---
+  Description:该launch启动文件是为了启动扩展板,使其进入正常工作状态.启动后就会在
+  /imu_data话题上发布imu传感器信息.需要该信息的节点,只需订阅该话题即可.同时我们还可
+  以使用dynamic_reconfigure来动态实时的修正z轴的角度.
+  Author: corvin
+  History:
+    20191031:Initial this launch file.
+-->
+<launch>
+  <arg name="imu_cfg_file" default="$(find rasp_imu_hat_6dof)/cfg/param.yaml"/>
+
+  <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
+      <rosparam file="$(arg imu_cfg_file)" command="load"/>
+  </node>
+</launch>
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/launch/imu_rviz_display.launch

@@ -0,0 +1,12 @@
+<!---
+  Description:该launch启动文件是为了在启动扩展板,使其进入正常工作状态后,
+    可以在rviz中可视化查看imu扩展板当前的姿态信息.
+  Author: corvin
+  History:
+    20191031:Initial this launch file.
+-->
+<launch>
+  <node pkg="rviz" type="rviz" name="imu_rviz_node"
+        args="-d $(find rasp_imu_hat_6dof)/cfg/imu_display.rviz">
+  </node>
+</launch>

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_data.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Author: corvin
+# Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
+#    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
+#    中读取IMU模块的三轴加速度、三轴角速度、四元数。
+# History:
+#    20191031: Initial this file.
+#    20191209：Add get imu chip temperature function-get_temp().
+#    20200702: 读取各数据后需要使用np.short转换才能进行后续数据处理.
+#    20210319:从寄存器地址读取数据时,直接一次性读取多个字节,不是依次
+#      读取多个寄存器地址,每个地址读取２字节.
+import smbus
+import rospy
+import numpy as np
+
+class MyIMU(object):
+    def __init__(self, addr):
+        self.addr = addr
+        self.i2c = smbus.SMBus(1)
+
+    def get_YPRAG(self):
+        try:
+            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
+            axyz_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
+            gxyz_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
+        except IOError:
+            rospy.logerr("Read IMU RPYAG date error !")
+        else:
+            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])/32768.0*180.0)
+            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
+            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)
+
+            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
+            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
+            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)
+
+            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
+            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
+            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)
+
+    def get_quatern(self):
+        try:
+            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
+        except IOError:
+            rospy.logerr("Read IMU quaternion date error !")
+        else:
+            self.raw_q0 = float((np.short(np.short(quat_data[1]<<8)|quat_data[0]))/32768.0)
+            self.raw_q1 = float((np.short(np.short(quat_data[3]<<8)|quat_data[2]))/32768.0)
+            self.raw_q2 = float((np.short(np.short(quat_data[5]<<8)|quat_data[4]))/32768.0)
+            self.raw_q3 = float((np.short(np.short(quat_data[7]<<8)|quat_data[6]))/32768.0)
+
+    def get_two_float(self, data, n):
+        data = str(data)
+        a, b, c = data.partition('.')
+        c = (c+"0"*n)[:n]
+        return ".".join([a,c])
+
+    def get_temp(self):
+        try:
+            temp = self.i2c.read_i2c_block_data(self.addr, 0x40, 2)
+        except IOError:
+            rospy.logerr("Read IMU temperature data error !")
+        else:
+            self.temp = float((temp[1]<<8)|temp[0])/100.0
+            #self.temp = float(self.get_two_float(self.temp, 2)) #keep 2 decimal places
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

@@ -0,0 +1,138 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Author: corvin
+# Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
+#    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
+#    中读取IMU模块的三轴加速度、角度、四元数，然后组装成ROS
+#    中的IMU消息格式，发布到/imu话题中，这样有需要的节点可以
+#    直接订阅该话题即可获取到imu扩展板当前的数据。
+# History:
+#    20191031:Initial this file.
+#    20191209:新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
+#    20200406:增加可以直接发布yaw数据的话题.
+#    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
+#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,
+#      否则加速度就为负值,单位m/s2.
+#    20210319:计算四元素时,不用自己使用rpy数据来计算,直接使用imu模块提供的.
+import rospy
+import math
+
+from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
+from dynamic_reconfigure.server import Server
+from rasp_imu_hat_6dof.cfg import imuConfig
+from std_msgs.msg import Float32
+from sensor_msgs.msg import Imu
+from imu_data import MyIMU
+
+
+degrees2rad = math.pi/180.0
+imu_yaw_calibration = 0.0
+yaw = 0.0
+
+# ros server return Yaw data
+def return_yaw_data(req):
+    return GetYawDataResponse(yaw)
+
+# Callback for dynamic reconfigure requests
+def reconfig_callback(config, level):
+    global imu_yaw_calibration
+    imu_yaw_calibration = config['yaw_calibration']
+    rospy.loginfo("Set imu_yaw_calibration to %f" % (imu_yaw_calibration))
+    return config
+
+rospy.init_node("imu_node")
+
+# Get DIY config param
+data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
+temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
+link_name  = rospy.get_param('~link_name', 'base_imu_link')
+yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
+pub_hz = rospy.get_param('~pub_hz', '20')
+
+data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
+temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
+yaw_pub = rospy.Publisher(yaw_topic_name, Float32, queue_size=1)
+srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
+yaw_srv = rospy.Service('imu_node/GetYawData', GetYawData, return_yaw_data)
+
+imuMsg = Imu()
+yawMsg = Float32()
+
+# Orientation covariance estimation
+imuMsg.orientation_covariance = [
+0.0025 , 0 , 0,
+0, 0.0025, 0,
+0, 0, 0.0025
+]
+
+# Angular velocity covariance estimation
+imuMsg.angular_velocity_covariance = [
+0.02, 0 , 0,
+0 , 0.02, 0,
+0 , 0 , 0.02
+]
+
+# linear acceleration covariance estimation
+imuMsg.linear_acceleration_covariance = [
+0.04 , 0 , 0,
+0 , 0.04, 0,
+0 , 0 , 0.04
+]
+
+seq=0
+# sensor accel g convert to m/s^2.
+accel_factor = 9.806
+
+myIMU = MyIMU(0x50)
+rate = rospy.Rate(pub_hz)
+
+rospy.loginfo("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)
+    yaw_deg = yaw_deg + imu_yaw_calibration
+    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
+    yaw_pub.publish(yawMsg)
+
+    # Publish imu message
+    #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
+    #              myIMU.raw_ay, myIMU.raw_az)
+    imuMsg.linear_acceleration.x = -float(myIMU.raw_ax)*accel_factor
+    imuMsg.linear_acceleration.y = -float(myIMU.raw_ay)*accel_factor
+    imuMsg.linear_acceleration.z = -float(myIMU.raw_az)*accel_factor
+
+    imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
+    imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
+    imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
+
+    # From IMU module get quatern param
+    myIMU.get_quatern()
+    imuMsg.orientation.x = myIMU.raw_q1
+    imuMsg.orientation.y = myIMU.raw_q2
+    imuMsg.orientation.z = myIMU.raw_q3
+    imuMsg.orientation.w = myIMU.raw_q0
+
+    imuMsg.header.stamp= rospy.Time.now()
+    imuMsg.header.frame_id = link_name
+    imuMsg.header.seq = seq
+    seq = seq + 1
+    data_pub.publish(imuMsg)
+
+    # Get imu module temperature, then publish to topic
+    myIMU.get_temp()
+    temp_pub.publish(myIMU.temp)
+
+    rate.sleep()
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/package.xml

@@ -0,0 +1,29 @@
+<package>
+  <name>rasp_imu_hat_6dof</name>
+  <version>1.2.0</version>
+  <description>
+     rasp_imu_hat_6dof is a package that provides a ROS driver
+      for the corvin 6dof imu hat.
+  </description>
+
+  <maintainer email="corvin_zhang@corvin.cn">Corvin Zhang</maintainer>
+
+  <license>BSD</license>
+  <author>Corvin Zhang</author>
+
+  <url type="website">https://www.corvin.cn</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>message_runtime</run_depend>
+</package>
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/srv/GetYawData.srv

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/CMakeLists.txt

@@ -0,0 +1,187 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(serial_imu_hat_6dof)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
+  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 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
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  setYawZero.srv
+  setBaudRate.srv
+  setPinOutHL.srv
+  getYawData.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+# )
+
+###################################
+## 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 serial_imu_hat_6dof
+#  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/serial_imu_hat_6dof.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_imu_node src/proc_serial_data.cpp src/serial_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(serial_imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(serial_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 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_imu_hat_6dof.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)

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/cfg/param.yaml

@@ -0,0 +1,55 @@
+###########################################################
+# Description:使用串口与IMU模块进行通信时的配置信息,可以根据需要修改
+# 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
+# 可以根据需要来修改,这里需要注意参数名称和参数之间要有空格隔开:
+#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
+#     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
+#     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
+#     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
+#     就需要在raspi-config中关闭串口登录功能,打开硬件
+#     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
+#  baud_rate:串口通信波特率,默认为57600,如果查看发布的
+#  　　话题中没有IMU数据可以尝试换波特率为9600或115200.
+#  pub_topic_hz:话题发布的数据频率,默认设置为100Hz.
+#  imu_link_name:imu模块的link名,该名称一般在urdf模型中定义.
+#  pub_data_topic:发布imu数据的ROS话题名.
+#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pitch_pub_topic:将pitch数据通过该话题发布出来.
+#  roll_pub_topic:将roll数据通过该话题发布出来.
+#  yaw_zero_topic:通过向该话题发布空消息即可将yaw角度归零.
+#  yaw_zero_service:通过向该服务发送空请求即可将yaw归零.
+#  baud_update_srv:通过向该服务发送1,2,3可以将波特率更新为
+#    9600bps,57600bps,115200bps,三种通信波特率.
+#  pin_outHL_srv:控制IMU板上D0-D4的输出电平高低的服务.
+#  yaw_data_srv:可以获取到yaw当前角度的服务.
+# Author: corvin
+# History:
+#   20200402:init this file.
+#   20200406:增加直接发布yaw数据的ROS话题.
+#   20210317:去掉数据位,奇偶校验,停止位参数,这几个
+#     参数模块已经固定死了,无需修改.
+#   20210318:增加yaw角度归零的话题配置参数yaw_zero_topic.
+#     增加使用服务方式将yaw归零的名称yaw_zero_service.
+#   20210319:增加修改串口波特率的配置参数baud_update_srv.
+#   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
+#     服务配置参数pin_outHL_srv.增加获取yaw数据的服务.
+#   20210910:更改默认话题发布imu数据的频率为50hz.
+#   20211003:更新发布imu数据话题的默认频率为100hz;
+#     新增发布俯仰角数据的话题参数:pitch_pub_topic;
+#     新增发布横滚角数据的话题参数:roll_pub_topic;
+###########################################################
+imu_dev: /dev/ttyAMA0
+baud_rate: 57600
+pub_topic_hz: 100
+
+imu_link_name: base_imu_link
+pub_data_topic: imu_data
+yaw_pub_topic: yaw_data
+pitch_pub_topic: pitch_data
+roll_pub_topic: roll_data
+yaw_zero_topic: yaw_zero_topic
+yaw_zero_service: yaw_zero_service
+baud_update_srv: baud_update_service
+pin_outHL_srv: pin_outHL_service
+yaw_data_srv: /imu_node/GetYawData

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/include/imu_data.h

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -0,0 +1,13 @@
+<!--
+  Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
+  Author: corvin
+  History:
+    20200401: init this file.
+-->
+<launch>
+  <arg name="imu_cfg_file" default="$(find serial_imu_hat_6dof)/cfg/param.yaml" />
+
+  <node pkg="serial_imu_hat_6dof" type="serial_imu_node" name="serial_imu_node" output="screen">
+    <rosparam file="$(arg imu_cfg_file)" command="load" />
+  </node>
+</launch>

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/package.xml

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -0,0 +1,214 @@
+/*****************************************************************
+ * Description:使用串口来读取IMU模块的数据,并通过ROS话题topic将
+ *   数据发布出来.芯片中默认读取出来的加速度数据单位是g,需要将其
+ *   转换为ROS中加速度规定的m/s2才能发布.
+ * Author: corvin
+ * History:
+ *   20200403:init this file.
+ *   20200406:增加发布yaw数据的话题.
+ *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
+ *     正方向时数据为正,否则为负.单位m/s2。
+ *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
+ *     修改的配置参数,因为都是固定的,无需自己修改配置.
+ *   20210318:增加订阅话题,话题消息格式Empty,可以将yaw角度归零.
+ *     增加使用service方式可将yaw角度归零.
+ *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
+ *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
+ *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
+ *     的服务,这里的高电平为3.3V.增加通过服务方式获取yaw数据.
+ *   20211003:增加发布pitch和roll数据的话题.
+ *   20220220:修改三轴加速度数据正负值,ROS中使用ENU坐标系,规定Z轴线
+ *     加速度的正方向为下,所以IMU静止时G为正值.
+*****************************************************************/
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <imu_data.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Empty.h>
+#include "serial_imu_hat_6dof/setYawZero.h"
+#include "serial_imu_hat_6dof/setBaudRate.h"
+#include "serial_imu_hat_6dof/setPinOutHL.h"
+#include "serial_imu_hat_6dof/getYawData.h"
+
+static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
+
+/***********************************************************
+ * Description:将yaw角度归零的话题回调函数,只要往话题中发布一条
+ *   std_msgs::Empty类型消息,即可将yaw角度归零.
+ **********************************************************/
+void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
+{
+    makeYawZero();
+}
+
+/******************************************************************
+ * Description:使用service方式来进行yaw角度归零,这里是服务的回调函数,
+ *   当有客户端发送yaw归零的服务时,自动调用该函数,如果正确执行了yaw归零,
+ *   则response反馈为0,如果为其他负数则表明yaw归零命令执行失败.
+ ******************************************************************/
+bool yawZeroService(serial_imu_hat_6dof::setYawZero::Request &req,
+                    serial_imu_hat_6dof::setYawZero::Response &res)
+{
+    res.status = makeYawZero();
+    return true;
+}
+
+/********************************************************************
+ * Description:使用服务调用方式来修改与IMU模块通信的波特率,这里根据
+ *   客户端发送过来的request来决定修改的波特率,请求内容对应的波特率:
+ *   请求发送1,修改波特率为9600;
+ *   请求发送2,修改波特率为57600;
+ *   请求发送3,修改波特率为115200;
+ *   当修改IMU模块的波特率成功后函数会返回0,若返回其他负值,则表示
+ *   修改波特率失败,可以重新调用服务来修改波特率.
+ *******************************************************************/
+bool setBaudService(serial_imu_hat_6dof::setBaudRate::Request &req,
+                    serial_imu_hat_6dof::setBaudRate::Response &res)
+{
+    res.status = setIMUBaudRate(req.index);
+    return true;
+}
+
+/********************************************************************
+ * Description:控制IMU板的D0,D1,D2,D3共4个引脚的输出数字高低电平信号,
+ *   这里的服务request一次性控制4个引脚的状态,对应引脚0,则输出低电平,
+ *   请求为1,则输出高电平.根据反馈状态status,可以得知控制的结果,如果
+ *   反馈0,表明控制成功,负值的话控制失败.这里的高电平为3.3V.
+ ********************************************************************/
+bool pinOutHLService(serial_imu_hat_6dof::setPinOutHL::Request &req,
+                     serial_imu_hat_6dof::setPinOutHL::Response &res)
+{
+    res.status = setIMUPinOutHL(req.D0, req.D1, req.D2, req.D3);
+    return true;
+}
+
+/**********************************************************************
+ * Description:通过service服务调用方式来获取到yaw角度信息.
+ **********************************************************************/
+bool getYawDataService(serial_imu_hat_6dof::getYawData::Request &req,
+                       serial_imu_hat_6dof::getYawData::Response &res)
+{
+    res.yaw = g_yawData;
+    return true;
+}
+
+int main(int argc, char **argv)
+{
+    float yaw, pitch, roll;
+    std::string imu_dev;
+    int baud = 0;
+
+    std::string imu_link_name;
+    std::string imu_topic_name;
+    std::string yaw_pub_topic;
+    std::string pitch_pub_topic;
+    std::string roll_pub_topic;
+    std::string yaw_zero_topic;
+    std::string yaw_zero_service;
+    std::string baud_update_service;
+    std::string pin_outHL_service;
+    std::string yaw_data_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配置文件，然后从配置文件中读取参数
+    ros::param::get("~imu_dev",          imu_dev);
+    ros::param::get("~baud_rate",        baud);
+    ros::param::get("~imu_link_name",    imu_link_name);
+    ros::param::get("~pub_topic_hz",     pub_topic_hz);
+    ros::param::get("~pub_data_topic",   imu_topic_name);
+    ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
+    ros::param::get("~yaw_zero_service", yaw_zero_service);
+    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
+    ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
+    ros::param::get("~roll_pub_topic",   roll_pub_topic);
+    ros::param::get("~baud_update_srv",  baud_update_service);
+    ros::param::get("~pin_outHL_srv",    pin_outHL_service);
+    ros::param::get("~yaw_data_srv",     yaw_data_service);
+
+    //初始化imu模块串口,根据设备号和波特率建立连接
+    int ret = initSerialPort(imu_dev.c_str(), baud);
+    if(ret < 0) //通过串口连接IMU模块失败
+    {
+        ROS_ERROR("init IMU module serial port error !");
+        closeSerialPort();
+        return -1;
+    }
+    ROS_INFO("Now IMU module is working...");
+
+    //定义四个服务,分别是更新波特率,yaw角度归零,控制引脚输出高低电平和获取yaw当前角度
+    ros::ServiceServer setBaudSrv= handle.advertiseService(baud_update_service, setBaudService);
+    ros::ServiceServer setyawSrv = handle.advertiseService(yaw_zero_service,  yawZeroService);
+    ros::ServiceServer pinOutSrv = handle.advertiseService(pin_outHL_service, pinOutHLService);
+    ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  getYawDataService);
+
+    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 2);
+    ros::Publisher pitch_pub = handle.advertise<std_msgs::Float32>(pitch_pub_topic, 2);
+    ros::Publisher roll_pub  = handle.advertise<std_msgs::Float32>(roll_pub_topic, 2);
+    ros::Rate loop_rate(pub_topic_hz);
+
+    sensor_msgs::Imu imu_msg;
+    imu_msg.header.frame_id = imu_link_name;
+    std_msgs::Float32 yaw_msg;
+    std_msgs::Float32 pitch_msg;
+    std_msgs::Float32 roll_msg;
+    while(ros::ok())
+    {
+        if(getImuData() < 0)
+            break;
+
+        imu_msg.header.stamp = ros::Time::now();
+        roll  = getAngle(0)*degree2Rad;
+        pitch = getAngle(1)*degree2Rad;
+        yaw   = getAngle(2)*degree2Rad;
+        if(yaw >= 3.1415926)
+            yaw -= 6.2831852;
+
+        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
+        yaw_msg.data   = yaw;
+        pitch_msg.data = pitch;
+        roll_msg.data  = roll;
+        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
+        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
+        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
+
+        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        imu_msg.orientation.x = getQuat(1);
+        imu_msg.orientation.y = getQuat(2);
+        imu_msg.orientation.z = getQuat(3);
+        imu_msg.orientation.w = getQuat(0);
+        imu_msg.orientation_covariance = {0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025};
+        //三轴角速度数据
+        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
+        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
+        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
+        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
+                                               0, 0.02, 0,
+                                               0, 0, 0.02};
+
+        //三轴线加速度数据,这里都为正值
+        imu_msg.linear_acceleration.x = getAcc(0)*acc_factor;
+        imu_msg.linear_acceleration.y = getAcc(1)*acc_factor;
+        imu_msg.linear_acceleration.z = getAcc(2)*acc_factor;
+        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
+                                                   0, 0.04, 0,
+                                                   0, 0, 0.04};
+
+        imu_pub.publish(imu_msg); //将imu数据通过话题发布出去
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    closeSerialPort(); //关闭与IMU模块的串口连接
+    return 0;
+}

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/getYawData.srv

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setBaudRate.srv

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setPinOutHL.srv

@@ -0,0 +1,6 @@
+int32 D0
+int32 D1
+int32 D2
+int32 D3
+---
+int32 status

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setYawZero.srv

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

src/robot_bringup/CMakeLists.txt

@@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+###################################
+## 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 robot_bringup
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/robot_bringup.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/robot_bringup_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_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
+# )
+
+## 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_robot_bringup.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)

src/robot_bringup/launch/odom_ekf.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+
+"""
+    Republish the /robot_pose_ekf/odom_combined topic which is of type
+    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
+    type nav_msgs/Odometry so we can view it in RViz.
+"""
+import rospy
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseWithCovarianceStamped
+
+class OdomEKF():
+    def __init__(self):
+        rospy.init_node('odom_ekf_node', anonymous=False)
+
+        # Publisher of type nav_msgs/Odometry
+        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
+
+        # Wait for the /odom_combined topic to become available
+        rospy.wait_for_message('input', PoseWithCovarianceStamped)
+
+        # Subscribe to the /odom_combined topic
+        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
+        rospy.loginfo("Publishing combined odometry on /odom_ekf")
+
+    def pub_ekf_odom(self, msg):
+        odom = Odometry()
+        odom.header = msg.header
+        odom.header.frame_id = '/odom_combined'
+        odom.child_frame_id = 'base_footprint'
+        odom.pose = msg.pose
+
+        self.ekf_pub.publish(odom)
+
+if __name__ == '__main__':
+    try:
+        OdomEKF()
+        rospy.spin()
+    except:
+        pass
+

src/robot_bringup/launch/robot_bringup.launch

@@ -0,0 +1,43 @@
+<!--
+  Description:robot启动时最先需要启动的launch文件,主要加载各基础功能模块启动.
+    首先就需要启动机器人的urdf文件,这样就可以在rviz中可视化显示机器人模型.然后
+    启动下位机arduino的程序,上下位机建立连接,这样上位机就可以发送控制命令.接下来
+    启动IMU模块,开始发布姿态信息.接下来就开始进行信息融合,使用imu信息和轮式里程计
+    这样得出的里程计更为准确.
+  Author: corvin
+  History:
+    20200716:init this file.
+    20210602:使用串口获取imu模块的数据.
+-->
+<launch>
+    <!-- (1) startup panda robot urdf description -->
+    <include file="$(find robot_description)/launch/robot_description.launch" />
+
+    <!-- (2) startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+    <!-- (3) startup rasp serial imu-6DOF board-->
+    <include file="$(find serial_imu_hat_6dof)/launch/serial_imu_hat.launch" />
+
+    <!-- (4) startup rasp cam-->
+    <include file="$(find rasp_camera)/launch/rasp_camera.launch" />
+
+    <!-- (5) startup robot_pose_ekf node -->
+    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+        <param name="output_frame" value="odom_combined"/>
+        <param name="freq" value="10.0"/>
+        <param name="sensor_timeout" value="1.2"/>
+        <param name="odom_used" value="true"/>
+        <param name="imu_used" value="true"/>
+        <param name="vo_used" value="false"/>
+        <param name="debug" value="false"/>
+        <param name="self_diagnose" value="false"/>
+    </node>
+
+    <!-- (6) /odom_combined view in rviz -->
+    <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+        <remap from="input" to="/robot_pose_ekf/odom_combined" />
+        <remap from="output" to="/odom_ekf" />
+    </node>
+</launch>
+