提交初始代码

README.md

@@ -1,4 +1,27 @@
-# blackTornadoRobot
+# <center>blackTornadoRobot</center>
+
+---
+
+## 前言
+ROS三轮全向移动小车“黑旋风”由[**ROS小课堂**](https://www.corvin.cn/)自主研发。
+本仓库为小车的上位机代码，包含语音、建图与导航、语音等功能包。
+
+---
+## 示例介绍
+本仓库为ROS入门教程的代码示例，包括以下ROS软件包:
+
+| 软件包 | 内容 |
+| :--- | :----: |
+| **infrared_maze** | 机器人利用红外传感器数据走迷宫 |
+| **rasp_imu_hat_6dof** | 6自由度imu模块 |
+| **razor_imu_9dof** | 9自由度imu模块 |
+| **robot_amcl** | 机器人导航功能包 |
+| **robot_bringup** | 启动机器人 |
+| **robot_calibration** | 标定机器人移动底盘 |
+| **robot_description** | 机器人urdf模型等 |
+| **robot_navigation** | 建图导航功能包，可实现建图导航功能 |
+| **ros_arduino_bridge** |  ros与arduino通信功能包  |
+| **teleop_twist_keyboard** | 键盘控制小车运动 |
+| **voice_system** | 机器人语音系统，包括语音控制小车运动，语音报警等功能 |
+
 
-ROS三轮全向移动小车“黑旋风”由[**ROS小课堂**](https://www.corvin.cn/)自主研发。
-本仓库为小车的上位机代码，包含语音、建图与导航、语音等功能包。

src/CMakeLists.txt

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

src/infrared_maze/CMakeLists.txt

@@ -0,0 +1,210 @@
+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
+  rasp_imu_hat_6dof
+)
+
+## 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,44 @@
+#####################################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用小车上三个红外测距传感器进行避障
+#   走迷宫,这里是一些可以配置的参数.
+#   infrared_topic:订阅的红外传感器话题.
+#   cmd_topic:控制移动的话题名.
+#   yaw_service:发布IMU模块yaw偏行角的服务.
+#   infrared_service:发布红外测据信息的服务名.
+#   linear_x:移动时前进速度大小.
+#   linear_y:横向移动时大小.
+#   angular_speed:原地转圈时角速度大小.
+#   warn_dist:小车是否可以前进的距离.
+#   tolerance_dist:距离容忍值.
+#   odom_frame:odom的tf名称.
+#   base_frame:底盘的tf名称.
+#   forward_dist:每次前进时移动的距离.
+# Author: corvin,adam
+# History:
+#   20200404:init this file.
+#   20200406:增加yaw service,rate参数.
+#   20200412:增加获取红外测距信息的service.
+#   20200424:更新注释,增加参数介绍.
+#   20200830:增加初始化参数.
+######################################################
+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.17
+linear_y: 0.15
+angular_speed: 0.5
+warn_dist: 0.25
+tolerance_dist: 0.05
+forward_dist: 0.50
+
+init_switch: false
+circle_dist: 0.18
+wall_dist: 0.16
+init_tolerance_dist: 0.01
+init_linear_x: 0.17
+init_linear_y: 0.05
+init_angular_speed: 0.2

src/infrared_maze/launch/infrared_maze.launch

@@ -0,0 +1,16 @@
+<!--
+  Copyright: 2016-2020 www.corvin.cn
+  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,650 @@
+/***************************************************************
+ Copyright(C): 2016-2020 ROS小课堂 www.corvin.cn
+ 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 <cmath>
+#include "serial_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  INIT_POSE   5
+#define  TURN_LEFT_10   6
+#define  TURN_RIGHT_10   7
+#define  VERTICAL_MOVE   8
+#define  TURN_BIAS   9
+
+#define  ERROR       -1
+
+//global variable
+ros::Publisher twist_pub;
+ros::ServiceClient yawSrvClient;
+serial_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; //每次前进移动时走的距离
+
+bool init_switch = false;
+float circle_dist = 0.18;//圆盘直径
+float wall_dist = 0.16;//小车处于中线位置时里wall的距离
+float init_tolerance_dist = 0.01;//平移时左右差的容忍值
+float angular_bias = 0.0;//偏离中线的弧度值
+float angular_bias_symbol = 1.0;
+
+
+float front_dist;
+float left_dist;
+float right_dist;
+
+std::string odomFrame;
+std::string baseFrame;
+
+float linear_x_speed = 0.17;
+float linear_y_speed = 0.15;
+float angular_speed = 0.2;
+
+float init_linear_x_speed = 0.17;
+float init_linear_y_speed = 0.15;
+float init_angular_speed = 0.2;
+
+float start_yaw_data = 0.0;
+int turnFlag = ERROR;
+
+/*********************************************************
+ * Descripition: 发布控制移动的消息,这里对于平面移动的
+ *   机器人只需要控制linear_x,linear_y和angular_z即可.
+ ********************************************************/
+void publishTwistCmd(float linear_x, float linear_y, float angular_z)
+{
+    geometry_msgs::Twist twist_msg;
+
+    twist_msg.linear.x = linear_x;
+    twist_msg.linear.y = linear_y;
+    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;
+    float angular_update_speed = 0.0;
+    angular_update_speed = angular_speed;
+    //开始计算修正后的目标角度
+    if((turnFlag == GO_FORWARD)||(turnFlag == HORIZ_MOVE)||(turnFlag == VERTICAL_MOVE))
+    {
+        target = start_yaw;
+        if(turnFlag == HORIZ_MOVE||turnFlag == VERTICAL_MOVE){
+            angular_update_speed = init_angular_speed;
+        }
+    }
+    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 if(turnFlag == TURN_BIAS)
+    {
+        if(angular_bias > 0){
+            target = start_yaw + angular_bias;
+            if(target > M_PI)
+            {
+                target = target - angular_bias;
+            }
+        }else{
+            target = start_yaw + angular_bias;
+            if(target < -M_PI)
+            {
+                target = target - angular_bias;
+            }
+        }
+        angular_update_speed = init_angular_speed;
+    }
+    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_update_speed;
+        }
+        else
+        {
+            angular = -angular_update_speed;
+        }
+        ROS_WARN("Yaw diff:%f, angular:%f", diff, angular);
+        publishTwistCmd(0, 0, angular);
+        ros::Duration(0.04).sleep();
+    }
+    publishTwistCmd(0, 0, 0); //在修正角度结束后应该立刻停止小车转动
+    ROS_INFO("Correct Yaw diff over !");
+    ros::Duration(0.1).sleep(); //100 ms
+}
+
+/****************************************************
+ * 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, 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, 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;
+            publishTwistCmd(linear_x_speed, 0, 0);         
+            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 TURN_LEFT_10:
+            turnFlag = GO_FORWARD;
+            angular = init_angular_speed;
+            check_angle = M_PI/18.0;
+            break;
+
+        case TURN_RIGHT_10:
+            turnFlag = GO_FORWARD;
+            angular = -init_angular_speed;
+            check_angle = M_PI/18.0;
+            break;
+
+        case TURN_BIAS:
+            turnFlag = TURN_BIAS;
+            check_angle = angular_bias;
+            if(angular_bias>0){
+                angular = init_angular_speed;
+            }else
+            {
+                angular = -init_angular_speed;
+                check_angle = -check_angle;   
+            }
+            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, 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, 0); //原地转弯完成后,需要立刻停止小车的旋转
+    ROS_INFO("Turning finish !!!");
+    ros::Duration(0.1).sleep(); //100 ms
+
+    return 0;
+}
+
+/**********************************************************
+ * Description:控制小车移动在过道中间,比较左右两边的距离后
+ *   然后进行水平左右移动.使小车保持在过道中间位置.
+ **********************************************************/
+void horizMoveMiddle(float tolerance)
+{
+    float diff = 0.0; //计算左右两边距离差
+    float begin_yaw = 0.0;
+    float now_left  = 0.0;
+    float now_right = 0.0;
+
+    turnFlag = HORIZ_MOVE;
+    yawSrvClient.call(yawSrv); //记录平移前的角度,作为标准进行修正
+    begin_yaw = yawSrv.response.yaw;
+
+    ros::Duration(0.02).sleep();//在获取两次传感器值之间需要加延迟
+    distSrvClient.call(distSrv);
+    now_left = distSrv.response.left_dist; //获取左边距离
+    ros::Duration(0.02).sleep();//在获取两次传感器值之间需要加延迟
+    distSrvClient.call(distSrv);
+    now_right = distSrv.response.right_dist;//获取右边距离
+
+    diff = now_left - now_right;
+    ROS_INFO("diff:%f,left:%f,right:%f", diff, now_left, now_right);
+    while((fabs(diff)>tolerance))
+    {
+        if(diff < 0) //move right
+        {
+            ROS_WARN("move Right horizontally !");
+            if(tolerance == tolerance_dist){
+                publishTwistCmd(0, -linear_y_speed, 0);
+            }else{
+                publishTwistCmd(0, -init_linear_y_speed, 0);
+            }
+            
+        }else if(diff > 0) //move left
+        {
+            ROS_WARN("move Left horizontally !");
+            if(tolerance == tolerance_dist){
+                publishTwistCmd(0, linear_y_speed, 0);
+            }else{
+                publishTwistCmd(0, init_linear_y_speed, 0);
+            }
+        }
+        
+
+        distSrvClient.call(distSrv);
+        now_left = distSrv.response.left_dist;
+        ros::Duration(0.02).sleep();//在获取两次传感器值之间需要加延迟
+        distSrvClient.call(distSrv);
+        now_right = distSrv.response.right_dist;
+
+        diff = now_left - now_right; //计算左右距离差值
+    }
+    //当平移结束后立刻停止移动
+    publishTwistCmd(0, 0, 0); //stop move
+    ros::Duration(0.1).sleep(); //delay 100ms
+    yawUpdate(begin_yaw); //用于修正转歪的角度
+}
+
+/***************************************************************************
+ * 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);
+        horizMoveMiddle(tolerance);
+        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 init_pose_angular(){
+    float begin_yaw = 0.0;
+    float now_left  = 0.0;
+    float next_left  = 0.0;
+    float now_right = 0.0;
+
+    yawSrvClient.call(yawSrv);
+    begin_yaw = yawSrv.response.yaw;
+    distSrvClient.call(distSrv);
+    now_left = distSrv.response.left_dist; //获取左边距离
+    
+    controlTurn(TURN_LEFT_10);
+    distSrvClient.call(distSrv);
+    next_left = distSrv.response.left_dist; //获取左边距离
+    if(next_left > now_left){
+        ROS_WARN("direction is left");
+        angular_bias_symbol = -1.0;
+    }else{
+        ROS_WARN("direction is right");
+    }
+
+    controlTurn(TURN_RIGHT_10);
+    //turnFlag = HORIZ_MOVE;
+    //yawUpdate(begin_yaw);
+    
+    distSrvClient.call(distSrv);
+    now_left = distSrv.response.left_dist; //获取左边距离
+    now_right = distSrv.response.right_dist;//获取右边距离
+    if(fabs(now_left-now_right)>init_tolerance_dist){
+            horizMoveMiddle(init_tolerance_dist);
+            ROS_WARN("horiz_move_middle");
+    }
+
+    yawSrvClient.call(yawSrv);
+    begin_yaw = yawSrv.response.yaw;
+    distSrvClient.call(distSrv);
+    now_left = distSrv.response.left_dist; //获取左边距离
+    now_right = distSrv.response.right_dist;//获取右边距离
+    angular_bias = angular_bias_symbol*acos(forward_dist/(now_left+circle_dist+now_right));
+    ROS_INFO("angular_bias:%f", angular_bias);
+
+    controlTurn(TURN_BIAS);
+    //turnFlag = TURN_BIAS;
+    yawUpdate(begin_yaw);
+
+    distSrvClient.call(distSrv);
+    now_left = distSrv.response.left_dist; //获取左边距离
+    now_right = distSrv.response.right_dist;//获取右边距离
+    if(fabs(now_left-now_right)>init_tolerance_dist){
+            horizMoveMiddle(init_tolerance_dist);
+            ROS_WARN("horiz_move_middle");
+    }
+
+    ROS_INFO("Init pose angular finished !");
+}
+
+/*****************************************************************************
+ * Description: 初始化机器人走迷宫时的位置..
+ ****************************************************************************/
+void init_pose_position(){
+    float diff = 0.0; 
+    float begin_yaw = 0.0;
+    float now_front  = 0.0;
+
+    turnFlag = VERTICAL_MOVE;
+    yawSrvClient.call(yawSrv); //记录垂直移动前的角度,作为标准进行修正
+    begin_yaw = yawSrv.response.yaw;
+
+    distSrvClient.call(distSrv);
+    now_front = distSrv.response.front_dist; //获取前边距离
+
+    diff = now_front - wall_dist;
+    while(fabs(diff)>tolerance_dist){
+        if(diff < 0) //move backward
+        {
+            ROS_WARN("move backward vertically !");
+            publishTwistCmd(-init_linear_x_speed, 0, 0);
+        }
+        else //move left
+        {
+            ROS_WARN("move forward  vertically !");
+            publishTwistCmd(init_linear_x_speed, 0, 0);
+        }
+        
+
+        distSrvClient.call(distSrv);
+        now_front = distSrv.response.front_dist;
+
+        diff = now_front - wall_dist; //计算前后距离差值
+    }
+
+    //当竖直结束后立刻停止移动
+    publishTwistCmd(0, 0, 0); //stop move
+    
+    ros::Duration(0.1).sleep(); //delay 100ms
+    yawUpdate(begin_yaw); //用于修正转歪的角度
+
+    ROS_INFO("Init pose position finished !");
+}
+
+/*****************************************************************************
+ * Description: 初始化机器人走迷宫时的位姿.
+ ****************************************************************************/
+void init_pose(){
+    float init_left_dist = 0.0;
+    float init_front_dist = 0.0;
+    float init_right_dist = 0.0;
+
+    distSrvClient.call(distSrv);
+    init_left_dist = distSrv.response.left_dist;
+    init_right_dist = distSrv.response.right_dist;
+    if(fabs(init_left_dist-init_right_dist)>init_tolerance_dist){
+            horizMoveMiddle(init_tolerance_dist);
+            ROS_WARN("horiz_move_middle");
+    }
+
+    init_pose_angular();
+    
+    distSrvClient.call(distSrv);
+    init_front_dist = distSrv.response.front_dist;
+    if(fabs(init_front_dist-wall_dist)>tolerance_dist){
+        init_pose_position();
+        ROS_WARN("init_pose_position");
+    }
+    
+}
+
+/*****************************************************************************
+ * 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("~linear_y",  linear_y_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::param::get("~circle_dist", circle_dist);
+    ros::param::get("~wall_dist", wall_dist);
+    ros::param::get("~init_tolerance_dist", init_tolerance_dist);
+    ros::param::get("~init_switch", init_switch);
+    ros::param::get("~init_linear_x",  init_linear_x_speed);
+    ros::param::get("~init_linear_y",  init_linear_y_speed);
+    ros::param::get("~init_angular_speed", init_angular_speed);
+
+    twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
+    yawSrvClient = handle.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
+    distSrvClient = handle.serviceClient<ros_arduino_msgs::GetInfraredDistance>(infrared_service);
+    
+    if(init_switch){
+        init_pose();
+    }
+    
+    ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
+
+    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
+  roscpp
+  sensor_msgs
+  cv_bridge
+  OpenCV 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
+
+## 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
+
+#############
+## 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="true"/>
+  <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,67 @@
+<?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>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/rasp_camera/scripts/functions.py

@@ -0,0 +1,61 @@
+# coding: utf-8
+import numpy as np
+
+
+def identity_function(x):
+    return x
+
+
+def step_function(x):
+    return np.array(x > 0, dtype=np.int)
+
+
+def sigmoid(x):
+    return 1 / (1 + np.exp(-x))    
+
+
+def sigmoid_grad(x):
+    return (1.0 - sigmoid(x)) * sigmoid(x)
+    
+
+def relu(x):
+    return np.maximum(0, x)
+
+
+def relu_grad(x):
+    grad = np.zeros(x)
+    grad[x>=0] = 1
+    return grad
+    
+
+def softmax(x):
+    if x.ndim == 2:
+        x = x.T
+        x = x - np.max(x, axis=0)
+        y = np.exp(x) / np.sum(np.exp(x), axis=0)
+        return y.T 
+
+    x = x - np.max(x) # 溢出对策
+    return np.exp(x) / np.sum(np.exp(x))
+
+
+def mean_squared_error(y, t):
+    return 0.5 * np.sum((y-t)**2)
+
+
+def cross_entropy_error(y, t):
+    if y.ndim == 1:
+        t = t.reshape(1, t.size)
+        y = y.reshape(1, y.size)
+        
+    # 监督数据是one-hot-vector的情况下，转换为正确解标签的索引
+    if t.size == y.size:
+        t = t.argmax(axis=1)
+             
+    batch_size = y.shape[0]
+    return -np.sum(np.log(y[np.arange(batch_size), t] + 1e-7)) / batch_size
+
+
+def softmax_loss(X, t):
+    y = softmax(X)
+    return cross_entropy_error(y, t)

src/rasp_camera/scripts/gradient.py

@@ -0,0 +1,53 @@
+# coding: utf-8
+import numpy as np
+
+def _numerical_gradient_1d(f, x):
+    h = 1e-4 # 0.0001
+    grad = np.zeros_like(x)
+    
+    for idx in range(x.size):
+        tmp_val = x[idx]
+        x[idx] = float(tmp_val) + h
+        fxh1 = f(x) # f(x+h)
+        
+        x[idx] = tmp_val - h 
+        fxh2 = f(x) # f(x-h)
+        grad[idx] = (fxh1 - fxh2) / (2*h)
+        
+        x[idx] = tmp_val # 还原值
+        
+    return grad
+
+
+def numerical_gradient_2d(f, X):
+    if X.ndim == 1:
+        return _numerical_gradient_1d(f, X)
+    else:
+        grad = np.zeros_like(X)
+        
+        for idx, x in enumerate(X):
+            grad[idx] = _numerical_gradient_1d(f, x)
+        
+        return grad
+
+
+def numerical_gradient(f, x):
+    h = 1e-4 # 0.0001
+    grad = np.zeros_like(x)
+    
+    # 多维迭代
+    it = np.nditer(x, flags=['multi_index'], op_flags=['readwrite'])
+    while not it.finished:
+        idx = it.multi_index
+        tmp_val = x[idx]
+        x[idx] = float(tmp_val) + h
+        fxh1 = f(x) # f(x+h)
+        
+        x[idx] = tmp_val - h 
+        fxh2 = f(x) # f(x-h)
+        grad[idx] = (fxh1 - fxh2) / (2*h)
+        
+        x[idx] = tmp_val # 还原值
+        it.iternext()   
+        
+    return grad

src/rasp_camera/scripts/layers.py

@@ -0,0 +1,284 @@
+# coding: utf-8
+import numpy as np
+from functions import *
+from util import im2col, col2im
+
+
+class Relu:
+    def __init__(self):
+        self.mask = None
+
+    def forward(self, x):
+        self.mask = (x <= 0)
+        out = x.copy()
+        out[self.mask] = 0
+
+        return out
+
+    def backward(self, dout):
+        dout[self.mask] = 0
+        dx = dout
+
+        return dx
+
+
+class Sigmoid:
+    def __init__(self):
+        self.out = None
+
+    def forward(self, x):
+        out = sigmoid(x)
+        self.out = out
+        return out
+
+    def backward(self, dout):
+        dx = dout * (1.0 - self.out) * self.out
+
+        return dx
+
+
+class Affine:
+    def __init__(self, W, b):
+        self.W =W
+        self.b = b
+        
+        self.x = None
+        self.original_x_shape = None
+        # 权重和偏置参数的导数
+        self.dW = None
+        self.db = None
+
+    def forward(self, x):
+        # 对应张量
+        self.original_x_shape = x.shape
+        x = x.reshape(x.shape[0], -1)
+        self.x = x
+
+        out = np.dot(self.x, self.W) + self.b
+
+        return out
+
+    def backward(self, dout):
+        dx = np.dot(dout, self.W.T)
+        self.dW = np.dot(self.x.T, dout)
+        self.db = np.sum(dout, axis=0)
+        
+        dx = dx.reshape(*self.original_x_shape)  # 还原输入数据的形状（对应张量）
+        return dx
+
+
+class SoftmaxWithLoss:
+    def __init__(self):
+        self.loss = None
+        self.y = None # softmax的输出
+        self.t = None # 监督数据
+
+    def forward(self, x, t):
+        self.t = t
+        self.y = softmax(x)
+        self.loss = cross_entropy_error(self.y, self.t)
+        
+        return self.loss
+
+    def backward(self, dout=1):
+        batch_size = self.t.shape[0]
+        if self.t.size == self.y.size: # 监督数据是one-hot-vector的情况
+            dx = (self.y - self.t) / batch_size
+        else:
+            dx = self.y.copy()
+            dx[np.arange(batch_size), self.t] -= 1
+            dx = dx / batch_size
+        
+        return dx
+
+
+class Dropout:
+    """
+    http://arxiv.org/abs/1207.0580
+    """
+    def __init__(self, dropout_ratio=0.5):
+        self.dropout_ratio = dropout_ratio
+        self.mask = None
+
+    def forward(self, x, train_flg=True):
+        if train_flg:
+            self.mask = np.random.rand(*x.shape) > self.dropout_ratio
+            return x * self.mask
+        else:
+            return x * (1.0 - self.dropout_ratio)
+
+    def backward(self, dout):
+        return dout * self.mask
+
+
+class BatchNormalization:
+    """
+    http://arxiv.org/abs/1502.03167
+    """
+    def __init__(self, gamma, beta, momentum=0.9, running_mean=None, running_var=None):
+        self.gamma = gamma
+        self.beta = beta
+        self.momentum = momentum
+        self.input_shape = None # Conv层的情况下为4维，全连接层的情况下为2维  
+
+        # 测试时使用的平均值和方差
+        self.running_mean = running_mean
+        self.running_var = running_var  
+        
+        # backward时使用的中间数据
+        self.batch_size = None
+        self.xc = None
+        self.std = None
+        self.dgamma = None
+        self.dbeta = None
+
+    def forward(self, x, train_flg=True):
+        self.input_shape = x.shape
+        if x.ndim != 2:
+            N, C, H, W = x.shape
+            x = x.reshape(N, -1)
+
+        out = self.__forward(x, train_flg)
+        
+        return out.reshape(*self.input_shape)
+            
+    def __forward(self, x, train_flg):
+        if self.running_mean is None:
+            N, D = x.shape
+            self.running_mean = np.zeros(D)
+            self.running_var = np.zeros(D)
+                        
+        if train_flg:
+            mu = x.mean(axis=0)
+            xc = x - mu
+            var = np.mean(xc**2, axis=0)
+            std = np.sqrt(var + 10e-7)
+            xn = xc / std
+            
+            self.batch_size = x.shape[0]
+            self.xc = xc
+            self.xn = xn
+            self.std = std
+            self.running_mean = self.momentum * self.running_mean + (1-self.momentum) * mu
+            self.running_var = self.momentum * self.running_var + (1-self.momentum) * var            
+        else:
+            xc = x - self.running_mean
+            xn = xc / ((np.sqrt(self.running_var + 10e-7)))
+            
+        out = self.gamma * xn + self.beta 
+        return out
+
+    def backward(self, dout):
+        if dout.ndim != 2:
+            N, C, H, W = dout.shape
+            dout = dout.reshape(N, -1)
+
+        dx = self.__backward(dout)
+
+        dx = dx.reshape(*self.input_shape)
+        return dx
+
+    def __backward(self, dout):
+        dbeta = dout.sum(axis=0)
+        dgamma = np.sum(self.xn * dout, axis=0)
+        dxn = self.gamma * dout
+        dxc = dxn / self.std
+        dstd = -np.sum((dxn * self.xc) / (self.std * self.std), axis=0)
+        dvar = 0.5 * dstd / self.std
+        dxc += (2.0 / self.batch_size) * self.xc * dvar
+        dmu = np.sum(dxc, axis=0)
+        dx = dxc - dmu / self.batch_size
+        
+        self.dgamma = dgamma
+        self.dbeta = dbeta
+        
+        return dx
+
+
+class Convolution:
+    def __init__(self, W, b, stride=1, pad=0):
+        self.W = W
+        self.b = b
+        self.stride = stride
+        self.pad = pad
+        
+        # 中间数据（backward时使用）
+        self.x = None   
+        self.col = None
+        self.col_W = None
+        
+        # 权重和偏置参数的梯度
+        self.dW = None
+        self.db = None
+
+    def forward(self, x):
+        FN, C, FH, FW = self.W.shape
+        N, C, H, W = x.shape
+        out_h = 1 + int((H + 2*self.pad - FH) / self.stride)
+        out_w = 1 + int((W + 2*self.pad - FW) / self.stride)
+
+        col = im2col(x, FH, FW, self.stride, self.pad)
+        col_W = self.W.reshape(FN, -1).T
+
+        out = np.dot(col, col_W) + self.b
+        out = out.reshape(N, out_h, out_w, -1).transpose(0, 3, 1, 2)
+
+        self.x = x
+        self.col = col
+        self.col_W = col_W
+
+        return out
+
+    def backward(self, dout):
+        FN, C, FH, FW = self.W.shape
+        dout = dout.transpose(0,2,3,1).reshape(-1, FN)
+
+        self.db = np.sum(dout, axis=0)
+        self.dW = np.dot(self.col.T, dout)
+        self.dW = self.dW.transpose(1, 0).reshape(FN, C, FH, FW)
+
+        dcol = np.dot(dout, self.col_W.T)
+        dx = col2im(dcol, self.x.shape, FH, FW, self.stride, self.pad)
+
+        return dx
+
+
+class Pooling:
+    def __init__(self, pool_h, pool_w, stride=1, pad=0):
+        self.pool_h = pool_h
+        self.pool_w = pool_w
+        self.stride = stride
+        self.pad = pad
+        
+        self.x = None
+        self.arg_max = None
+
+    def forward(self, x):
+        N, C, H, W = x.shape
+        out_h = int(1 + (H - self.pool_h) / self.stride)
+        out_w = int(1 + (W - self.pool_w) / self.stride)
+
+        col = im2col(x, self.pool_h, self.pool_w, self.stride, self.pad)
+        col = col.reshape(-1, self.pool_h*self.pool_w)
+
+        arg_max = np.argmax(col, axis=1)
+        out = np.max(col, axis=1)
+        out = out.reshape(N, out_h, out_w, C).transpose(0, 3, 1, 2)
+
+        self.x = x
+        self.arg_max = arg_max
+
+        return out
+
+    def backward(self, dout):
+        dout = dout.transpose(0, 2, 3, 1)
+        
+        pool_size = self.pool_h * self.pool_w
+        dmax = np.zeros((dout.size, pool_size))
+        dmax[np.arange(self.arg_max.size), self.arg_max.flatten()] = dout.flatten()
+        dmax = dmax.reshape(dout.shape + (pool_size,)) 
+        
+        dcol = dmax.reshape(dmax.shape[0] * dmax.shape[1] * dmax.shape[2], -1)
+        dx = col2im(dcol, self.x.shape, self.pool_h, self.pool_w, self.stride, self.pad)
+        
+        return dx

src/rasp_camera/scripts/line_follow.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+'''
+Author: adam_zhuo
+Date: 2020-10-21 16:31:05
+LastEditTime: 2021-01-13 16:06:33
+LastEditors: Please set LastEditors
+Description: In User Settings Edit
+FilePath: /blackTornadoRobot/src/rasp_camera/scripts/line_follow.py
+'''
+
+import rospy, cv2, cv_bridge, numpy, math, datetime
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+
+class Follower:
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        # cv2.namedWindow('window', 1)
+
+        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+
+
+        self.twist = Twist()
+        self.err = 0
+        self.cnt = 0
+
+        # self.lift_height_pub.publish(20)
+
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+
+        cv2.imshow('window_img',img)
+        cv2.waitKey(3)
+
+        
+
+        # ROI
+        # cropped = img[y0:y1, x0:x1]
+        cropped = img[0:140, 20:300]
+
+        cv2.imshow('window_cropped',cropped)
+        cv2.waitKey(3)
+
+        # Binarization
+        gray = cv2.cvtColor(cropped, cv2.COLOR_RGB2GRAY)
+
+        cv2.imshow('window_gray', gray)
+        cv2.waitKey(3)
+
+        ret, binimg = cv2.threshold(gray, 90, 255, cv2.THRESH_BINARY)
+
+        cv2.imshow('window_BIN', binimg)
+        cv2.waitKey(3)
+
+        # calculate center of bounding box
+        y, x = (binimg < 125).nonzero()
+
+        # Judge center of line
+        x0 = 140
+        if x.shape[0] != 0:
+            y0 = (max(y) + min(y))/2
+            x0 = (max(x) + min(x))/2
+            # print '(x0,y0):', x0, y0
+
+        # print 'x.shape[0]:',  x.shape[0]
+        if x.shape[0] < 100:
+            self.cnt  += 1
+            if self.cnt > 20:
+                self.twist.linear.x = 0.0
+                self.twist.angular.z = 0.0
+                print 'Stop!'
+        else:    
+            self.cnt = 0
+            # control
+            err = x0 - 140
+            print 'err:', err
+            self.twist.linear.x = 0.08
+            self.twist.angular.z = 0.0
+            if abs(err) > 5:
+                self.twist.angular.z = 0.74 * -err * 0.006
+                print 'angular.z', self.twist.angular.z
+                print 'Turn'
+            else:
+                print 'Go ahead!'
+        self.cmd_vel_pub.publish(self.twist)
+        # self.lift_height_pub.publish(20)
+
+
+if __name__ == '__main__':
+    rospy.init_node('follow')
+    follow = Follower()
+    rospy.spin()

src/rasp_camera/scripts/mnist_recognition.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+import sys, os,math
+import numpy as np
+from PIL import Image as im
+
+from simple_convnet import SimpleConvNet
+from functions import softmax
+
+import rospy, cv2, cv_bridge
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+network = SimpleConvNet(input_dim=(1,28,28), 
+                        conv_param = {'filter_num': 30, 'filter_size': 5, 'pad': 0, 'stride': 1},
+                        hidden_size=100, output_size=10, weight_init_std=0.01)
+network.load_params("/home/corvin/blackTornadoRobot/src/rasp_camera/scripts/params.pkl")
+
+ball_color = 'black'
+
+color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
+              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
+              'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
+              'black': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])},
+              'yellow': {'Lower': np.array([21, 39, 64]), 'Upper': np.array([34, 255, 255])},
+              }
+
+class Mnist_Recognition:
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
+        self.result_pub = rospy.Publisher('/voice_system/iflytek_offline_tts_topic',String,queue_size=1)
+        self.result_string = String()
+        self.confidence_threshold = 0.95
+        self.last_resut = -1
+
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        cv2.imshow('window_img',img)
+        cv2.waitKey(3)
+
+        gs_frame = cv2.GaussianBlur(img, (5, 5), 0)
+        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像
+        erode_hsv = cv2.erode(hsv, None, iterations=2)                   # 腐蚀 粗的变细
+        inRange_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
+        
+        cv2.imshow('window_inrange_img',inRange_hsv)
+        cv2.waitKey(3)
+
+        cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+        if len(cnts) == 0:
+            print("no region")
+            return
+        c = max(cnts, key=cv2.contourArea)
+        rect = cv2.minAreaRect(c)
+        box = cv2.boxPoints(rect)
+        cv2.drawContours(img, [np.int0(box)], -1, (0, 255, 255), 2)
+
+        cv2.imshow('window_contours', img)
+        cv2.waitKey(3)
+
+        y0 = int(math.floor(min(box[0,1],box[1,1],box[2,1],box[3,1])))-10
+        y1 = int(math.ceil(max(box[0,1],box[1,1],box[2,1],box[3,1])))+10
+        x0 = int(math.floor(min(box[0,0],box[1,0],box[2,0],box[3,0])))-10
+        x1 = int(math.ceil(max(box[0,0],box[1,0],box[2,0],box[3,0])))+10
+
+        x_err = x1 - x0
+        y_err = y1 - y0
+        if y_err > x_err :
+            err = y_err - x_err
+            x0 = x0 - err/2
+            x1 = x1 + err/2
+        else :
+            err = x_err - y_err
+            y0 = y0 - err/2
+            y1 = y1 + err/2
+        if x0<0 or y0<0 or x0 >320 or y0 >240:
+            print("move number")
+            return
+
+        cropped = inRange_hsv[y0:y1, x0:x1]
+        cv2.imshow('window_cropped', cropped)
+        cv2.waitKey(3)
+
+        resized_img = cv2.resize(cropped,(28,28),interpolation = cv2.INTER_AREA)
+        cv2.imshow('window_resizedimg',resized_img)
+        cv2.waitKey(3)
+
+        img_array = resized_img.reshape(1,1,28, 28) / 255.0
+
+        __result = network.predict(img_array)  
+        __result = softmax(__result)
+        result = np.argmax(__result)          # 预测的数字
+        confidence = __result[0,result]
+        if confidence < self.confidence_threshold:
+            print("unvalid result")
+            return
+        print("result and confidence is ",result,confidence)
+        if result != self.last_resut:
+            self.result_string.data = str(result)
+            self.result_pub.publish(self.result_string)
+            self.last_resut = result
+        
+
+
+if __name__ == '__main__':
+    rospy.init_node('mnist_recognition')
+    mnist_recognition = Mnist_Recognition()
+    rospy.spin()

src/rasp_camera/scripts/simple_convnet.py

@@ -0,0 +1,160 @@
+# coding: utf-8
+import sys, os
+sys.path.append(os.pardir)  # 为了导入父目录的文件而进行的设定
+import pickle
+import numpy as np
+from collections import OrderedDict
+from layers import *
+from gradient import numerical_gradient
+
+
+class SimpleConvNet:
+    """简单的ConvNet
+
+    conv - relu - pool - affine - relu - affine - softmax
+    
+    Parameters
+    ----------
+    input_size : 输入大小（MNIST的情况下为784）
+    hidden_size_list : 隐藏层的神经元数量的列表（e.g. [100, 100, 100]）
+    output_size : 输出大小（MNIST的情况下为10）
+    activation : 'relu' or 'sigmoid'
+    weight_init_std : 指定权重的标准差（e.g. 0.01）
+        指定'relu'或'he'的情况下设定“He的初始值”
+        指定'sigmoid'或'xavier'的情况下设定“Xavier的初始值”
+    """
+    def __init__(self, input_dim=(1, 28, 28), 
+                 conv_param={'filter_num':30, 'filter_size':5, 'pad':0, 'stride':1},
+                 hidden_size=100, output_size=10, weight_init_std=0.01):
+        filter_num = conv_param['filter_num']
+        filter_size = conv_param['filter_size']
+        filter_pad = conv_param['pad']
+        filter_stride = conv_param['stride']
+        input_size = input_dim[1]
+        conv_output_size = (input_size - filter_size + 2*filter_pad) / filter_stride + 1
+        pool_output_size = int(filter_num * (conv_output_size/2) * (conv_output_size/2))
+
+        # 初始化权重
+        self.params = {}
+        self.params['W1'] = weight_init_std * \
+                            np.random.randn(filter_num, input_dim[0], filter_size, filter_size)
+        self.params['b1'] = np.zeros(filter_num)
+        self.params['W2'] = weight_init_std * \
+                            np.random.randn(pool_output_size, hidden_size)
+        self.params['b2'] = np.zeros(hidden_size)
+        self.params['W3'] = weight_init_std * \
+                            np.random.randn(hidden_size, output_size)
+        self.params['b3'] = np.zeros(output_size)
+
+        # 生成层
+        self.layers = OrderedDict()
+        self.layers['Conv1'] = Convolution(self.params['W1'], self.params['b1'],
+                                           conv_param['stride'], conv_param['pad'])
+        self.layers['Relu1'] = Relu()
+        self.layers['Pool1'] = Pooling(pool_h=2, pool_w=2, stride=2)
+        self.layers['Affine1'] = Affine(self.params['W2'], self.params['b2'])
+        self.layers['Relu2'] = Relu()
+        self.layers['Affine2'] = Affine(self.params['W3'], self.params['b3'])
+
+        self.last_layer = SoftmaxWithLoss()
+
+    def predict(self, x):
+        for layer in self.layers.values():
+            x = layer.forward(x)
+
+        return x
+
+    def loss(self, x, t):
+        """求损失函数
+        参数x是输入数据、t是教师标签
+        """
+        y = self.predict(x)
+        return self.last_layer.forward(y, t)
+
+    def accuracy(self, x, t, batch_size=100):
+        if t.ndim != 1 : t = np.argmax(t, axis=1)
+        
+        acc = 0.0
+        
+        for i in range(int(x.shape[0] / batch_size)):
+            tx = x[i*batch_size:(i+1)*batch_size]
+            tt = t[i*batch_size:(i+1)*batch_size]
+            y = self.predict(tx)
+            y = np.argmax(y, axis=1)
+            acc += np.sum(y == tt) 
+        
+        return acc / x.shape[0]
+
+    def numerical_gradient(self, x, t):
+        """求梯度（数值微分）
+
+        Parameters
+        ----------
+        x : 输入数据
+        t : 教师标签
+
+        Returns
+        -------
+        具有各层的梯度的字典变量
+            grads['W1']、grads['W2']、...是各层的权重
+            grads['b1']、grads['b2']、...是各层的偏置
+        """
+        loss_w = lambda w: self.loss(x, t)
+
+        grads = {}
+        for idx in (1, 2, 3):
+            grads['W' + str(idx)] = numerical_gradient(loss_w, self.params['W' + str(idx)])
+            grads['b' + str(idx)] = numerical_gradient(loss_w, self.params['b' + str(idx)])
+
+        return grads
+
+    def gradient(self, x, t):
+        """求梯度（误差反向传播法）
+
+        Parameters
+        ----------
+        x : 输入数据
+        t : 教师标签
+
+        Returns
+        -------
+        具有各层的梯度的字典变量
+            grads['W1']、grads['W2']、...是各层的权重
+            grads['b1']、grads['b2']、...是各层的偏置
+        """
+        # forward
+        self.loss(x, t)
+
+        # backward
+        dout = 1
+        dout = self.last_layer.backward(dout)
+
+        layers = list(self.layers.values())
+        layers.reverse()
+        for layer in layers:
+            dout = layer.backward(dout)
+
+        # 设定
+        grads = {}
+        grads['W1'], grads['b1'] = self.layers['Conv1'].dW, self.layers['Conv1'].db
+        grads['W2'], grads['b2'] = self.layers['Affine1'].dW, self.layers['Affine1'].db
+        grads['W3'], grads['b3'] = self.layers['Affine2'].dW, self.layers['Affine2'].db
+
+        return grads
+        
+    def save_params(self, file_name="params.pkl"):
+        params = {}
+        for key, val in self.params.items():
+            params[key] = val
+        with open(file_name, 'wb') as f:
+            pickle.dump(params, f)
+
+    def load_params(self, file_name="params.pkl"):
+        with open(file_name, 'rb') as f:
+            params = pickle.load(f)
+        for key, val in params.items():
+            self.params[key] = val
+
+        for i, key in enumerate(['Conv1', 'Affine1', 'Affine2']):
+            self.layers[key].W = self.params['W' + str(i+1)]
+            self.layers[key].b = self.params['b' + str(i+1)]

src/rasp_camera/scripts/util.py

@@ -0,0 +1,99 @@
+# coding: utf-8
+import numpy as np
+
+
+def smooth_curve(x):
+    """用于使损失函数的图形变圆滑
+
+    参考：http://glowingpython.blogspot.jp/2012/02/convolution-with-numpy.html
+    """
+    window_len = 11
+    s = np.r_[x[window_len-1:0:-1], x, x[-1:-window_len:-1]]
+    w = np.kaiser(window_len, 2)
+    y = np.convolve(w/w.sum(), s, mode='valid')
+    return y[5:len(y)-5]
+
+
+def shuffle_dataset(x, t):
+    """打乱数据集
+
+    Parameters
+    ----------
+    x : 训练数据
+    t : 监督数据
+
+    Returns
+    -------
+    x, t : 打乱的训练数据和监督数据
+    """
+    permutation = np.random.permutation(x.shape[0])
+    x = x[permutation,:] if x.ndim == 2 else x[permutation,:,:,:]
+    t = t[permutation]
+
+    return x, t
+
+def conv_output_size(input_size, filter_size, stride=1, pad=0):
+    return (input_size + 2*pad - filter_size) / stride + 1
+
+
+def im2col(input_data, filter_h, filter_w, stride=1, pad=0):
+    """
+
+    Parameters
+    ----------
+    input_data : 由(数据量, 通道, 高, 长)的4维数组构成的输入数据
+    filter_h : 滤波器的高
+    filter_w : 滤波器的长
+    stride : 步幅
+    pad : 填充
+
+    Returns
+    -------
+    col : 2维数组
+    """
+    N, C, H, W = input_data.shape
+    out_h = (H + 2*pad - filter_h)//stride + 1
+    out_w = (W + 2*pad - filter_w)//stride + 1
+
+    img = np.pad(input_data, [(0,0), (0,0), (pad, pad), (pad, pad)], 'constant')
+    col = np.zeros((N, C, filter_h, filter_w, out_h, out_w))
+
+    for y in range(filter_h):
+        y_max = y + stride*out_h
+        for x in range(filter_w):
+            x_max = x + stride*out_w
+            col[:, :, y, x, :, :] = img[:, :, y:y_max:stride, x:x_max:stride]
+
+    col = col.transpose(0, 4, 5, 1, 2, 3).reshape(N*out_h*out_w, -1)
+    return col
+
+
+def col2im(col, input_shape, filter_h, filter_w, stride=1, pad=0):
+    """
+
+    Parameters
+    ----------
+    col :
+    input_shape : 输入数据的形状（例：(10, 1, 28, 28)）
+    filter_h :
+    filter_w
+    stride
+    pad
+
+    Returns
+    -------
+
+    """
+    N, C, H, W = input_shape
+    out_h = (H + 2*pad - filter_h)//stride + 1
+    out_w = (W + 2*pad - filter_w)//stride + 1
+    col = col.reshape(N, out_h, out_w, C, filter_h, filter_w).transpose(0, 3, 4, 5, 1, 2)
+
+    img = np.zeros((N, C, H + 2*pad + stride - 1, W + 2*pad + stride - 1))
+    for y in range(filter_h):
+        y_max = y + stride*out_h
+        for x in range(filter_w):
+            x_max = x + stride*out_w
+            img[:, :, y:y_max:stride, x:x_max:stride] += col[:, :, y, x, :, :]
+
+    return img[:, :, pad:H + pad, pad:W + pad]

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/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,93 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_complementary_filter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,60 @@
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
+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.2</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,207 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_filter_madgwick
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,75 @@
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
+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/COPYING

@@ -0,0 +1,165 @@
+                   GNU LESSER 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.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser 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
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.

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/package.xml

@@ -0,0 +1,44 @@
+<package>
+  <name>imu_filter_madgwick</name>
+  <version>1.2.2</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/rviz_imu_plugin/CHANGELOG.rst

@@ -0,0 +1,92 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rviz_imu_plugin
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,67 @@
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
+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.2</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,20 @@
+###################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# 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,17 @@
+<!---
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  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,13 @@
+<!---
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  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,69 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# 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,139 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+# 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,46 @@
+###########################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:使用串口与IMU模块进行通信时的配置信息.
+# 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
+# 可以根据需要来修改:
+#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
+#     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
+#     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
+#     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
+#     就需要在raspi-config中关闭串口登录功能,打开硬件
+#     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
+#  baud_rate:串口通信波特率,默认为57600,如果查看发布的
+#  　　话题中没有IMU数据可以尝试换波特率.
+#  pub_data_topic:发布imu数据的ROS话题名.
+#  pub_temp_topic:发布imu模块温度的ROS话题名.
+#  link_name:imu模块的link名.
+#  pub_hz:话题发布的数据频率,默认设置为30Hz.
+#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  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数据的服务.
+###########################################################
+imu_dev: /dev/ttyAMA0
+baud_rate: 57600
+pub_hz: 30
+
+link_name: base_imu_link
+pub_data_topic: imu_data
+pub_temp_topic: imu_temp
+yaw_pub_topic: yaw_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,26 @@
+/*******************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:操作imu模块的代码头文件.
+ * Author: corvin
+ * History:
+ *   20210318:init this file.
+ *   20210319:增加修改串口通信波特率的函数.
+ *   20210321:增加控制IMU引脚数字高低电平的函数.
+ *******************************************************/
+#ifndef _IMU_DATA_H_
+#define _IMU_DATA_H_
+
+int initSerialPort(const char* path, const int baud);
+
+int getImuData();
+int closeSerialPort();
+
+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,14 @@
+<!--
+  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
+  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,76 @@
+<?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>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</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>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</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 @@
+/*******************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * 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>
+
+
+static unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
+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()
+{
+    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[100];
+    static unsigned char chrCnt = 0;
+
+    signed short sData[4]; //save 8 Byte valid info
+    unsigned char i = 0;
+    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]))/32768.0*16.0;
+            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*16.0;
+            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*16.0;
+            break;
+
+        case 0x52: //角速度输出
+            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*2000.0;
+            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*2000.0;
+            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*2000.0;
+            break;
+
+        case 0x53: //欧拉角度输出, roll, pitch, yaw
+            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*180.0;
+            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*180.0;
+            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*180.0;
+            break;
+
+        case 0x59: //四元素输出
+            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
+            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
+            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
+            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
+            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
+            break;
+    }
+    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 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)
+    {
+        ROS_ERROR("openSerialPort error !");
+        return -1;
+    }
+
+    return 0;
+}
+/*****************************************
+ * Description:得到三轴加速度信息,输入
+ * 　参数可以为0,1,2分别代表x,y,z轴的
+ * 　加速度信息.
+ *****************************************/
+float getAcc(int flag)
+{
+    return acce[flag];
+}
+
+/******************************************
+ * Description:得到角速度信息,输入参数可
+ * 　以为0,1,2,分别代表x,y,z三轴的角速度
+ *   信息.
+ *****************************************/
+float getAngular(int flag)
+{
+    return angular[flag];
+}
+
+/*******************************************
+ * Description:获取yaw,pitch,roll,输入参数0,
+ * 1,2可以分别获取到roll,pitch,yaw的数据.
+ *******************************************/
+float getAngle(int flag)
+{
+    return angle[flag];
+}
+
+/********************************************
+ * Description:输入参数0,1,2,3可以分别获取到
+ * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
+ * 　的imu_msg.orientation.w,x,y,z的顺序,
+ * 　这里的q0是对应w参数,1,2,3对应x,y,z.
+ ********************************************/
+float getQuat(int flag)
+{
+    return quater[flag];
+}
+
+/*************************************************************
+ * Description:从串口读取数据,然后解析出各有效数据段,这里
+ *  一次性从串口中读取88个字节,然后需要从这些字节中进行
+ *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+int getImuData(void)
+{
+    int data_len = 0;
+    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
+
+    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
+    data_len = read(port_fd, r_buf, sizeof(r_buf));
+    if(data_len <= 0)
+    {
+        ROS_ERROR("read serial port data failed !\n");
+        closeSerialPort();
+        return -1;
+    }
+
+    //printf("recv data len:%d\n", data_len);
+    for (int i=0; i<data_len; i++)
+    {
+        //printf("0x%02x ", r_buf[i]);
+        parse_serialData(r_buf[i]);
+    }
+    //printf("\n\n");
+
+    return 0;
+}
+

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -0,0 +1,203 @@
+/*****************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * 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数据.
+*****************************************************************/
+#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角度归零的话题回调函数,只要往话题中
+ *   发布一条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;
+}
+
+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 link_name;
+    std::string imu_topic;
+    std::string temp_topic;
+    std::string yaw_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_hz = 0;
+    float degree2Rad = 3.1415926/180.0;
+    float acc_factor = 9.806;
+
+    ros::init(argc, argv, "imu_data_pub_node");
+    ros::NodeHandle handle;
+
+    //从yaml配置文件中读取配置参数
+    ros::param::get("~imu_dev",   imu_dev);
+    ros::param::get("~baud_rate", baud);
+    ros::param::get("~pub_hz",    pub_hz);
+    ros::param::get("~link_name", link_name);
+    ros::param::get("~pub_data_topic",   imu_topic);
+    ros::param::get("~pub_temp_topic",   temp_topic);
+    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("~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)
+    {
+        ROS_ERROR("init serial port error !");
+        closeSerialPort();
+        return -1;
+    }
+    ROS_INFO("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, 1);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 1);
+    ros::Rate loop_rate(pub_hz);
+
+    sensor_msgs::Imu imu_msg;
+    imu_msg.header.frame_id = link_name;
+    std_msgs::Float32 yaw_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值,可以通过服务来得到该值
+        //publish yaw data to topic
+        yaw_msg.data = yaw;
+        yaw_pub.publish(yaw_msg);
+
+        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        imu_msg.orientation.x = getQuat(1);
+        imu_msg.orientation.y = getQuat(2);
+        imu_msg.orientation.z = getQuat(3);
+        imu_msg.orientation.w = getQuat(0);
+        imu_msg.orientation_covariance = {0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025};
+
+        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
+        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
+        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
+        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
+                                               0, 0.02, 0,
+                                               0, 0, 0.02};
+
+        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAcc(0), getAcc(1), getAcc(2));
+        imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
+        imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
+        imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
+        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
+                                                   0, 0.04, 0,
+                                                   0, 0, 0.04};
+
+        imu_pub.publish(imu_msg);
+        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/razor_imu_9dof/.gitignore

@@ -0,0 +1 @@
+*~

src/razor_imu_9dof/CHANGELOG.rst

@@ -0,0 +1,69 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package razor_imu_9dof
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.1 (02-07-2016)
+------------------
+* Passing razor_config_file as ros parameter in launch file (Daniel Koguciuk)
+
+1.1.0 (08-03-2015)
+------------------
+* Resolving bug in exiting display_3D_visualization (`#15 <https://github.com/KristofRobot/razor_imu_9dof/issues/15>`_)
+* Adding dynamic reconfigure for yaw calibration (Paul Bouchier)
+* Moving calibration values from firmware to ROS yaml file (`#13 <https://github.com/KristofRobot/razor_imu_9dof/issues/13>`_)
+
+    * Note: this is a BREAKING CHANGE - requires firmware update (updated firmware provided)
+    
+* Refactoring code: moved scripts to nodes, renamed node.py to imu_node.py (Paul Bouchier)
+* Adding diagnostic status reporting (Paul Bouchier)
+
+1.0.5 (15-11-2014)
+------------------
+* Moving scripts from nodes to scripts dir
+* Installing files in ``src`` and ``magnetometer_calibration``
+* Major cleanup of package.xml and CMakeLists.txt
+
+1.0.4 (15-11-2014)
+------------------
+* Adding press 'a' to align feature
+* Moving magnetometer calibration sketches under dedicated ``magnetometer_calibration`` directory
+* Adding magnetometer calibration sketches for Processing and Matlab (Paul Bouchier)
+* Setting default USB port to /dev/ttyUSB0 in all files
+* Adding graceful exit in case USB port not found
+* Adding queue_size=1
+* Fixing x linear accelerations sign
+
+1.0.3 (02-11-2014)
+------------------
+* Moving all file one directory up
+* Changing axis orientation in 3D visualization to be in line with REP 103
+* Additional output of linear accelerationa and angular velocity in 3D visualization 
+* Adding units of measurement to 3D visualization
+* Major graphical improvements to the 3D visualization
+* Adding explanation on different launch files
+
+1.0.2 (31-10-2014)
+------------------
+* Adding valid covariances (Paul Bouchier)
+* Fixing incorrect direction of yaw & pitch orientation (Paul Bouchier)
+* Fix Readme references & instructions (Paul Bouchier)
+* Converting acceleration to m/s^2 (Paul Bouchier)
+* Adapting to new output message YPRAG instead of YPRAMG (Paul Bouchier)
+* Updating package.xml links to ahrs site (Paul Bouchier)
+* Upgrading Arduino package to 1.4.2 from Peter Bartz' site (Paul Bouchier)
+* Fixing link to Peter Bartz' site (Paul Bouchier)
+* Documenting #ox output mode (Paul Bouchier)
+* Renaming new mode #define to better reflect what it does (Paul Bouchier) 
+* Adding #ox output mode back into Peter's code (Paul Bouchier)
+
+1.0.1 (15-03-2014)
+------------------
+* Cleaning up code based on catkin_lint report
+* Creating additional launch files for display/publishing
+* Implementing flush of first IMU results
+* Changing default port, and adding output showing which port was selected
+* Removing obsolete ``roslib`` import and ``roslib.load_manifest``
+
+1.0.0 (29-12-2013)
+------------------
+* First catkinized version

src/razor_imu_9dof/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(razor_imu_9dof)
+
+find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure)
+
+generate_dynamic_reconfigure_options(cfg/imu.cfg)
+
+catkin_package(DEPENDS dynamic_reconfigure)
+
+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}
+)
+
+install(DIRECTORY magnetometer_calibration
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS 
+	nodes/imu_node.py
+	nodes/display_3D_visualization.py
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
+)

src/razor_imu_9dof/README.md

@@ -0,0 +1,124 @@
+Official ROS Documentation
+--------------------------
+A much more extensive and standard ROS-style version of this documentation can be found on the ROS wiki at:
+
+http://wiki.ros.org/razor_imu_9dof
+
+
+Install and Configure ROS Package
+---------------------------------
+1) Install dependencies:
+
+	``$ sudo apt-get install python-visual``
+
+2) Download code:
+
+	``$ cd ~/catkin_workspace/src``
+
+	``$ git clone https://github.com/blmhemu/razor_imu_9dof.git``
+
+	``$ cd ..``
+	
+	``$ catkin_make``
+
+
+Install Arduino firmware
+-------------------------
+1) Open ``src/Razor_AHRS/Razor_AHRS.ino`` in Arduino IDE. Note: this is a modified version
+of Peter Bartz' original Arduino code (see https://github.com/ptrbrtz/razor-9dof-ahrs). 
+Use this version - it emits linear acceleration and angular velocity data required by the ROS Imu message
+
+2) Select your hardware here by uncommenting the right line in ``src/Razor_AHRS/Razor_AHRS.ino``, e.g.
+
+<pre>
+// HARDWARE OPTIONS
+/*****************************************************************/
+// Select your hardware here by uncommenting one line!
+//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
+//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
+#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
+</pre>
+
+3) Upload Arduino sketch to the Sparkfun 9DOF Razor IMU board
+
+
+Configure
+---------
+In its default configuration, ``razor_imu_9dof`` expects a yaml config file ``my_razor.yaml`` with:
+* USB port to use
+* Calibration parameters
+
+An example``razor.yaml`` file is provided.
+Copy that file to ``my_razor.yaml`` as follows:
+
+    $ roscd razor_imu_9dof/config
+    $ cp razor.yaml my_razor.yaml
+
+Then, edit ``my_razor.yaml`` as needed
+
+Launch
+------
+Publisher and 3D visualization:
+
+	$ roslaunch razor_imu_9dof razor-pub-and-display.launch
+
+Publisher only:
+
+	$ roslaunch razor_imu_9dof razor-pub.launch
+
+Publisher only with diagnostics:
+
+	$ roslaunch razor_imu_9dof razor-pub-diags.launch
+
+3D visualization only:
+
+	$ roslaunch razor_imu_9dof razor-display.launch
+
+Conventions
+-----------
+
+The IMU follows ENU Convention.
+
+East  = 0 degree
+
+North = 90 degree
+
+West  = 180 degree / -180 degree
+
+South = -90 degree
+
+Accelerations and angular accelerations are also taken care of. (Right Hand Rule)
+
+This standard is in lines with ROS REP 105. This driver can be used for direct interfacing with robot_localization node.
+
+The setup used is shown below for reference
+
+![alt text](https://raw.githubusercontent.com/blmhemu/razor_imu_9dof/indigo-devel/razor.png)
+
+Calibrate
+---------
+For best accuracy, follow the tutorial to calibrate the sensors:
+
+http://wiki.ros.org/razor_imu_9dof
+
+A copy of Peter Bartz's magnetometer calibration scripts from https://github.com/ptrbrtz/razor-9dof-ahrs is provided in the ``magnetometer_calibration`` directory.
+
+Update ``my_razor.yaml`` with the new calibration parameters.
+
+Dynamic Reconfigure
+-------------------
+After having launched the publisher with one of the launch commands listed above, 
+it is possible to dynamically reconfigure the yaw calibration.
+
+1) Run:
+
+    $ rosrun rqt_reconfigure rqt_reconfigure 
+    
+2) Select ``imu_node``. 
+
+3) Change the slider to move the calibration +/- 10 degrees. 
+If you are running the 3D visualization you'll see the display jump when the new calibration takes effect.
+
+The intent of this feature is to let you tune the alignment of the AHRS to the direction of the robot driving direction, so that if you can determine that, for example, the AHRS reads 30 degrees when the robot is actually going at 35 degrees as shown by e.g. GPS, you can tune the calibration to make it read 35. It's the compass-equivalent of bore-sighting a camera.

src/razor_imu_9dof/cfg/imu.cfg

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

src/razor_imu_9dof/config/my_razor.yaml

@@ -0,0 +1,35 @@
+## USB port
+#port: /dev/IMU
+port: /dev/ttyUSB0
+
+
+##### Calibration ####
+### accelerometer
+accel_x_min: -250.0
+accel_x_max: 250.0
+accel_y_min: -250.0
+accel_y_max: 250.0
+accel_z_min: -250.0
+accel_z_max: 250.0
+
+### magnetometer
+# standard calibration
+magn_x_min: -600.0
+magn_x_max: 600.0
+magn_y_min: -600.0
+magn_y_max: 600.0
+magn_z_min: -600.0
+magn_z_max: 600.0
+
+# extended calibration
+calibration_magn_use_extended: false
+magn_ellipsoid_center: [0, 0, 0]
+magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
+
+# AHRS to robot calibration
+imu_yaw_calibration: 0.0
+
+### gyroscope
+gyro_average_offset_x: 0.0
+gyro_average_offset_y: 0.0
+gyro_average_offset_z: 0.0

src/razor_imu_9dof/config/razor.yaml

@@ -0,0 +1,34 @@
+## USB port
+port: /dev/ttyUSB0
+
+
+##### Calibration ####
+### accelerometer
+accel_x_min: -250.0
+accel_x_max: 250.0
+accel_y_min: -250.0
+accel_y_max: 250.0
+accel_z_min: -250.0
+accel_z_max: 250.0
+
+### magnetometer
+# standard calibration
+magn_x_min: -600.0
+magn_x_max: 600.0
+magn_y_min: -600.0
+magn_y_max: 600.0
+magn_z_min: -600.0
+magn_z_max: 600.0
+
+# extended calibration
+calibration_magn_use_extended: false
+magn_ellipsoid_center: [0, 0, 0]
+magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
+
+# AHRS to robot calibration
+imu_yaw_calibration: 0.0
+
+### gyroscope
+gyro_average_offset_x: 0.0
+gyro_average_offset_y: 0.0
+gyro_average_offset_z: 0.0

src/razor_imu_9dof/config/razor_diags.yaml

@@ -0,0 +1,7 @@
+pub_rate: 1.0
+analyzers:
+  Razor9DofImu:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: 'Imu'
+    timeout: 5.0
+    contains: ['Razor_Imu']

src/razor_imu_9dof/launch/razor-display.launch

@@ -0,0 +1,6 @@
+<launch>
+
+  <node pkg="razor_imu_9dof" type="display_3D_visualization.py" name="display_3D_visualization_node" output="screen">
+  </node>
+  
+</launch>