push imu part

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,88 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_complementary_filter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(imu_complementary_filter)
+
+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}
+)
+
+## 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
+)
+
+
+# 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 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.1</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,200 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_filter_madgwick
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,68 @@
+cmake_minimum_required(VERSION 2.8.3)
+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 signals)
+
+# 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 imu_filter_nodelet imu_filter_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_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,104 @@
+/*
+ *  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);
+};
+
+#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,115 @@
+/*
+ *  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_;
+    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,45 @@
+<package>
+  <name>imu_filter_madgwick</name>
+  <version>1.2.1</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>Ivan Dryanovski</author>
+  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
+  <maintainer email="ivan.dryanovski@gmail.com">Ivan Dryanovski</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,311 @@
+/*
+ *  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() :
+    q0(1.0), q1(0.0), q2(0.0), q3(0.0),
+    w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
+    zeta_ (0.0), gain_ (0.0), world_frame_(WorldFrame::ENU)
+{
+}
+
+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 recipNorm;
+  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);
+}

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,377 @@
+/*
+ *  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 ("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_);
+
+  // **** 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_;
+
+  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, invH, invA;
+
+  // A: pointing up
+  float Ax = A.x, Ay = A.y, Az = A.z;
+
+  // E: pointing down/north
+  float Ex = E.x, Ey = E.y, Ez = E.z;
+
+  // H: vector horizontal, pointing east
+  // H = E x A
+  crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
+
+  // normalize H
+  normH = normalizeVector(Hx, Hy, Hz);
+  if (normH < 1E-7) {
+    // device is close to free fall (or in space?), or close to
+    // magnetic north pole.
+    // mag in T => Threshold 1E-7, typical values are  > 1E-5.
+    return false;
+  }
+
+  // normalize A
+  normalizeVector(Ax, Ay, Az);
+
+  // M: vector horizontal, pointing north
+  // M = A x H
+  crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
+
+  // Create matrix for basis transformation
+  tf2::Matrix3x3 R;
+  switch (frame) {
+    case WorldFrame::NED:
+      // vector space world W:
+      // Basis: bwx (1,0,0) north, bwy (0,1,0) east, bwz (0,0,1) down
+      // vector space local L:
+      // Basis: M ,H, -A
+      // W(1,0,0) => L(M)
+      // W(0,1,0) => L(H)
+      // W(0,0,1) => L(-A)
+
+      // R: Transform Matrix local => world equals basis of L, because basis of W is I
+      R[0][0] = Mx;     R[0][1] = Hx;     R[0][2] = -Ax;
+      R[1][0] = My;     R[1][1] = Hy;     R[1][2] = -Ay;
+      R[2][0] = Mz;     R[2][1] = Hz;     R[2][2] = -Az;
+      break;
+
+    case WorldFrame::NWU:
+      // vector space world W:
+      // Basis: bwx (1,0,0) north, bwy (0,1,0) west, bwz (0,0,1) up
+      // vector space local L:
+      // Basis: M ,H, -A
+      // W(1,0,0) => L(M)
+      // W(0,1,0) => L(-H)
+      // W(0,0,1) => L(A)
+
+      // R: Transform Matrix local => world equals basis of L, because basis of W is I
+      R[0][0] = Mx;     R[0][1] = -Hx;     R[0][2] = Ax;
+      R[1][0] = My;     R[1][1] = -Hy;     R[1][2] = Ay;
+      R[2][0] = Mz;     R[2][1] = -Hz;     R[2][2] = Az;
+      break;
+
+    default:
+    case WorldFrame::ENU:
+      // vector space world W:
+      // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up
+      // vector space local L:
+      // Basis: H, M , A
+      // W(1,0,0) => L(H)
+      // W(0,1,0) => L(M)
+      // W(0,0,1) => L(A)
+
+      // R: Transform Matrix local => world equals basis of L, because basis of W is I
+      R[0][0] = Hx;     R[0][1] = Mx;     R[0][2] = Ax;
+      R[1][0] = Hy;     R[1][1] = My;     R[1][2] = Ay;
+      R[2][0] = Hz;     R[2][1] = Mz;     R[2][2] = Az;
+      break;
+  }
+
+  // Matrix.getRotation assumes vector rotation, but we're using
+  // coordinate systems. Thus negate rotation angle (inverse).
+  tf2::Quaternion q;
+  R.getRotation(q);
+  tf2::convert(q.inverse(), orientation);
+  return true;
+}
+
+
+bool StatelessOrientation::computeOrientation(
+  WorldFrame::WorldFrame frame,
+  geometry_msgs::Vector3 A,
+  geometry_msgs::Quaternion& orientation) {
+
+  // This implementation could be optimized regarding speed.
+
+  // magnetic Field E must not be parallel to A,
+  // choose an arbitrary orthogonal vector
+  geometry_msgs::Vector3 E;
+  if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) {
+      E.x = A.y;
+      E.y = A.x;
+      E.z = 0.0;
+  } else if (fabs(A.z) > 0.1) {
+      E.x = 0.0;
+      E.y = A.z;
+      E.z = A.y;
+  } else {
+      // free fall
+      return false;
+  }
+
+  return computeOrientation(frame, A, E, orientation);
+}

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

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

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

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

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

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/CHANGELOG.rst

@@ -0,0 +1,70 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_tools
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.2.1 (2019-05-06)
+------------------
+
+1.2.0 (2018-05-25)
+------------------
+
+1.1.5 (2017-05-24)
+------------------
+
+1.1.4 (2017-05-22)
+------------------
+
+1.1.3 (2017-03-10)
+------------------
+
+1.1.2 (2016-09-07)
+------------------
+
+1.1.1 (2016-09-07)
+------------------
+
+1.1.0 (2016-04-25)
+------------------
+
+1.0.11 (2016-04-22)
+-------------------
+
+1.0.10 (2016-04-22)
+-------------------
+
+1.0.9 (2015-10-16)
+------------------
+
+1.0.8 (2015-10-07)
+------------------
+* Add imu_complementary_filter to meta package
+* Contributors: Martin Günther
+
+1.0.7 (2015-10-07)
+------------------
+
+1.0.6 (2015-10-06)
+------------------
+
+1.0.5 (2015-06-24)
+------------------
+
+1.0.4 (2015-05-06)
+------------------
+
+1.0.3 (2015-01-29)
+------------------
+
+1.0.2 (2015-01-27)
+------------------
+
+1.0.1 (2014-12-10)
+------------------
+* add me as maintainer to package.xml
+* Contributors: Martin Günther
+
+1.0.0 (2014-09-03)
+------------------
+* First public release
+* catkinization of imu_tools metapackage
+* Contributors: Francisco Vina

src/rasp_imu_hat_6dof/imu_tools/imu_tools/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/CHANGELOG.rst

@@ -0,0 +1,86 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rviz_imu_plugin
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,64 @@
+cmake_minimum_required(VERSION 2.8.3)
+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})
+
+## 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,29 @@
+<package>
+  <name>rviz_imu_plugin</name>
+  <version>1.2.1</version>
+  <description>
+    RVIZ plugin for IMU visualization
+  </description>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/rviz_imu_plugin</url>
+  <author>Ivan Dryanovski</author>
+  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
+  <maintainer email="ivan.dryanovski@gmail.com">Ivan Dryanovski</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,178 @@
+/*
+ * 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,15 @@
+##################################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用IIC总线来读取IMU模块的数据,并
+#   通过话题将imu数据发送出来.这里是可以配置的
+#   一些参数.
+# 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-2019 https://www.corvin.cn ROS小课堂
+  Author: corvin
+  Description:该launch启动文件是为了启动扩展板，使其进入正常工作状态。启动后，就会在
+    /imu话题上发布imu传感器信息。需要该信息的节点，订阅该话题即可。同时，我们还可以使
+    用dynamic_reconfigure来动态的修正z轴的角度。
+  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-2019 https://www.corvin.cn ROS小课堂
+  Author: corvin
+  Description:该launch启动文件是为了在启动扩展板，使其进入正常工作状态后。
+    可以在rviz中可视化imu扩展板当前的姿态信息。
+  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,84 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2019 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转换才能进行后续数据处理.
+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:
+            roll_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 2)
+            pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
+            yaw_tmp   = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
+
+            ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
+            ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
+            az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
+            #rospy.loginfo("Read i2c accX:" + str(ax_tmp))
+            #rospy.loginfo("Read i2c accY:" + str(ay_tmp))
+            #rospy.loginfo("Read i2c accZ:" + str(az_tmp))
+
+            gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
+            gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
+            gz_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x39, 2)
+        except IOError:
+            rospy.logerr("Read IMU YPRAG date error !")
+        else:
+            self.raw_roll  = float(((roll_tmp[1]<<8) |roll_tmp[0])/32768.0*180.0)
+            self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
+            self.raw_yaw  = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
+
+            self.raw_ax = float(np.short(np.short(ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
+            self.raw_ay = float(np.short(np.short(ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
+            self.raw_az = float(np.short(np.short(az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
+            #rospy.loginfo("Read raw accX:" + str(self.raw_ax))
+            #rospy.loginfo("Read raw accY:" + str(self.raw_ay))
+            #rospy.loginfo("Read raw accZ:" + str(self.raw_az))
+
+            self.raw_gx = float(np.short(np.short(gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
+            self.raw_gy = float(np.short(np.short(gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
+            self.raw_gz = float(np.short(np.short(gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
+
+    def get_quatern(self):
+        try:
+            q0 = self.i2c.read_i2c_block_data(self.addr, 0x51, 2)
+            q1 = self.i2c.read_i2c_block_data(self.addr, 0x52, 2)
+            q2 = self.i2c.read_i2c_block_data(self.addr, 0x53, 2)
+            q3 = self.i2c.read_i2c_block_data(self.addr, 0x54, 2)
+        except IOError:
+            rospy.logerr("Read IMU quaternion date error !")
+        else:
+            self.raw_q0 = float((np.short((q0[1]<<8)|q0[0]))/32768.0)
+            self.raw_q1 = float((np.short((q1[1]<<8)|q1[0]))/32768.0)
+            self.raw_q2 = float((np.short((q2[1]<<8)|q2[0]))/32768.0)
+            self.raw_q3 = float((np.short((q3[1]<<8)|q3[0]))/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,145 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+# Copyright: 2016-2020 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.
+import rospy
+import math
+
+from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
+from tf.transformations import quaternion_from_euler
+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')
+pub_hz = rospy.get_param('~pub_hz', '20')
+yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
+
+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
+
+    # Calculate quatern param from roll,pitch,yaw by ourselves
+    q = quaternion_from_euler(roll, pitch, yaw)
+    imuMsg.orientation.x = q[0]
+    imuMsg.orientation.y = q[1]
+    imuMsg.orientation.z = q[2]
+    imuMsg.orientation.w = q[3]
+
+    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,206 @@
+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
+)
+
+## 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
+#   sensor_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 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(${PROJECT_NAME}_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 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_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,32 @@
+################################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用串口进行通信时的配置信息.
+#   imu_dev:指示串口设备挂在点,如果使用usb转串口
+#     模块,都是写ttyUSB0,1,2等.如果使用树莓派的
+#     GPIO排针处串口,则使用ttyS0.
+#   baud_rate:串口通信波特率,旧模块的波特率为9600,
+#     新版本的波特率为57600,如果串口没数据可以修改.
+#   data_bits:数据位
+#   parity:数据校验位
+#   stop_bits:停止位
+#   pub_data_topic:发布imu数据的话题名.
+#   pub_temp_topic:发布imu模块温度的话题名.
+#   link_name: imu模块的link名.
+#   pub_hz:话题发布的数据频率.
+# Author: corvin
+# History:
+#   20200402: init this file.
+#   20200406: 增加直接发布yaw数据的话题.
+################################################
+imu_dev: /dev/ttyS0
+baud_rate: 57600
+data_bits: 8
+parity: N
+stop_bits: 1
+
+pub_data_topic: imu_data
+pub_temp_topic: imu_temp
+yaw_topic: yaw_data
+link_name: base_imu_link
+pub_hz: 10
+

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/include/imu_data.h

@@ -0,0 +1,22 @@
+#ifndef _IMU_DATA_H_
+#define _IMU_DATA_H_
+
+int initSerialPort(const char* path, const int baud, const int dataBits,
+        const char* parity, const int stopBit);
+
+int getImuData();
+int closeSerialPort();
+
+float getAccX();
+float getAccY();
+float getAccZ();
+
+float getAngularX();
+float getAngularY();
+float getAngularZ();
+
+float getAngleX();
+float getAngleY();
+float getAngleZ();
+
+#endif

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -0,0 +1,14 @@
+<!--
+  Copyright: 2016-2020 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,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>serial_imu_hat_6dof</name>
+  <version>0.0.0</version>
+  <description>The serial_imu_hat_6dof package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/serial_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_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+
+  <!-- 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,327 @@
+/*****************************************************************
+ * Copyright:2016-2020 www.corvin.cn ROS小课堂
+ * Description:使用串口方式读取IMU模块信息.
+ * Author: corvin
+ * History:
+ *   20200401:init this file.
+ *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
+ *     这样才能进行/32768.0*16.0运算.
+******************************************************************/
+#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>
+
+
+char r_buf[512];
+int port_fd = 0;
+float acce[3],angular[3],angle[3],quater[4];
+
+/**********************************************
+ * Description:打开串口,输入各参数即可.
+ *********************************************/
+int openSerialPort(const char *path, int baud, int dataBits,
+                const char* parity, int stopBit)
+{
+    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", path);
+        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 19200:
+           cfsetispeed(&terminfo, B19200);
+           cfsetospeed(&terminfo, B19200);
+           break;
+
+        case 38400:
+           cfsetispeed(&terminfo, B38400);
+           cfsetospeed(&terminfo, B38400);
+           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;
+    }
+
+    //设置数据位
+    terminfo.c_cflag |= CLOCAL|CREAD;
+    terminfo.c_cflag &= ~CSIZE;
+    switch(dataBits)
+    {
+        case 7:
+           terminfo.c_cflag |= CS7;
+           break;
+
+        case 8:
+           terminfo.c_cflag |= CS8;
+           break;
+
+        default:
+           ROS_ERROR("set dataBit error !");
+           return -3;
+    }
+
+    //设置奇偶校验位
+    switch(*parity)
+    {
+        case 'o': //设置奇校验
+        case 'O':
+           terminfo.c_cflag |= PARENB;
+           terminfo.c_cflag |= PARODD;
+           terminfo.c_iflag |= (INPCK|ISTRIP);
+           break;
+
+        case 'e': //设置偶校验
+        case 'E':
+           terminfo.c_iflag |= (INPCK|ISTRIP);
+           terminfo.c_cflag |= PARENB;
+           terminfo.c_cflag &= ~PARODD;
+           break;
+
+        case 'n': //不设置奇偶校验位
+        case 'N':
+           terminfo.c_cflag &= ~PARENB;
+           break;
+
+        default:
+           ROS_ERROR("set parity error !");
+          return -4;
+    }
+
+    //设置停止位
+    switch(stopBit)
+    {
+        case 1:
+            terminfo.c_cflag &= ~CSTOPB;
+            break;
+
+        case 2:
+            terminfo.c_cflag |= CSTOPB;
+            break;
+
+        default:
+            ROS_ERROR("set stopBit error !");
+            return -5;
+    }
+
+    terminfo.c_cc[VTIME] = 10;
+    terminfo.c_cc[VMIN]  = 0;
+    tcflush(fd, TCIFLUSH);
+
+    if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
+    {
+        ROS_ERROR("set imu serial port attr error !");
+        return -6;
+    }
+
+    return fd;
+}
+
+/**************************************
+ * Description:关闭串口文件描述符.
+ *************************************/
+int closeSerialPort()
+{
+    int ret = close(port_fd);
+    return ret;
+}
+
+/*****************************************************
+ * Description:向串口中发送数据.
+ ****************************************************/
+int send_data(int fd, char *send_buffer, int length)
+{
+	length = write(fd, send_buffer, length*sizeof(unsigned char));
+
+	return length;
+}
+
+/*******************************************************
+ * Description:从串口中接收数据.
+ *******************************************************/
+int recv_data(int fd, char* recv_buffer, int len)
+{
+    int length = read(fd, recv_buffer, len);
+    return length;
+}
+
+/************************************************************
+ * Description:根据串口数据协议来解析有效数据,
+ ***********************************************************/
+void parse_serialData(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;
+    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;
+    }
+
+    //for(i=0;i<11; i++)
+    //  printf("0x%x ",chrBuf[i]);
+    //printf("\n");
+
+    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;
+            break;
+    }
+    chrCnt = 0;
+}
+
+/*****************************************************************
+ * Description:根据串口配置信息来配置打开串口,准备进行数据通信
+ ****************************************************************/
+int initSerialPort(const char* path, const int baud, const int dataBits,
+          const char* parity, const int stopBit)
+{
+    bzero(r_buf, 512);
+    port_fd = openSerialPort(path, baud, dataBits, parity, stopBit);
+    if(port_fd < 0)
+    {
+        ROS_ERROR("openSerialPort error !");
+        return -1;
+    }
+
+    return 0;
+}
+
+float getAccX()
+{
+    return acce[0];
+}
+float getAccY()
+{
+    return acce[1];
+}
+float getAccZ()
+{
+    return acce[2];
+}
+
+float getAngularX()
+{
+    return angular[0];
+}
+
+float getAngularY()
+{
+    return angular[1];
+}
+
+float getAngularZ()
+{
+    return angular[2];
+}
+
+float getAngleX()
+{
+    return angle[0];
+}
+float getAngleY()
+{
+    return angle[1];
+}
+float getAngleZ()
+{
+    return angle[2];
+}
+
+int getImuData()
+{
+    int ret = 0;
+    ret = recv_data(port_fd, r_buf,  44);
+    if(ret == -1)
+    {
+        ROS_ERROR("read serial port data failed !\n");
+        closeSerialPort();
+
+        return -1;
+    }
+
+    for (int i=0; i<ret; i++)
+    {
+        parse_serialData(r_buf[i]);
+    }
+
+    return 0;
+}

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -0,0 +1,113 @@
+/****************************************************
+ * Copyright:2016-2020 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU模块的数据,并通过
+ *   topic将数据发布出来.芯片中默认读取出来的加速度数据
+ *   单位是g,需要将其转换为ROS中加速度规定的m/s2才能发布.
+ * Author: corvin
+ * History:
+ *   20200403: init this file.
+ *   20200406:增加发布yaw数据的话题.
+ *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
+ *     正方向时数据为正,否则为负.单位m/s2。
+*****************************************************/
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include "imu_data.h"
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Float32.h>
+
+
+int main(int argc, char **argv)
+{
+    float yaw, pitch, roll;
+    std::string imu_dev;
+    int baud = 0;
+    int dataBit = 0;
+    std::string parity;
+    int stopBit = 0;
+
+    std::string link_name;
+    std::string imu_topic;
+    std::string temp_topic;
+    std::string yaw_topic;
+    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;
+
+    ros::param::get("~imu_dev", imu_dev);
+    ros::param::get("~baud_rate", baud);
+    ros::param::get("~data_bits", dataBit);
+    ros::param::get("~parity", parity);
+    ros::param::get("~stop_bits", stopBit);
+
+    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_topic", yaw_topic);
+    ros::param::get("~pub_hz", pub_hz);
+
+    int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
+    if(ret < 0)
+    {
+        ROS_ERROR("init serial port error !");
+        closeSerialPort();
+        return -1;
+    }
+    ROS_INFO("IMU module is working... ");
+
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_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  = getAngleX()*degree2Rad;
+        pitch = getAngleY()*degree2Rad;
+        yaw   = getAngleZ()*degree2Rad;
+        if(yaw >= 3.1415926)
+            yaw -= 6.2831852;
+
+        //publish yaw data
+        yaw_msg.data = yaw;
+        yaw_pub.publish(yaw_msg);
+
+        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        imu_msg.orientation_covariance = {0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025};
+
+        imu_msg.angular_velocity.x = getAngularX()*degree2Rad;
+        imu_msg.angular_velocity.y = getAngularY()*degree2Rad;
+        imu_msg.angular_velocity.z = getAngularZ()*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",getAccX(), getAccY(), getAccZ());
+        imu_msg.linear_acceleration.x = -getAccX()*acc_factor;
+        imu_msg.linear_acceleration.y = -getAccY()*acc_factor;
+        imu_msg.linear_acceleration.z = -getAccZ()*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();
+    return 0;
+}
+

src/robot_bringup/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## 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 robot_bringup
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/robot_bringup.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/robot_bringup_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_bringup.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/robot_bringup/launch/odom_ekf.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+""" odom_ekf.py - Version 1.1 2013-12-20
+
+    Republish the /robot_pose_ekf/odom_combined topic which is of type
+    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
+    type nav_msgs/Odometry so we can view it in RViz.
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    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 2 of the License, or
+    (at your option) any later version.5
+
+    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 at:
+
+    http://www.gnu.org/licenses/gpl.html
+"""
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from nav_msgs.msg import Odometry
+
+class OdomEKF():
+    def __init__(self):
+        rospy.init_node('odom_ekf_node', anonymous=False)
+
+        # Publisher of type nav_msgs/Odometry
+        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
+
+        # Wait for the /odom_combined topic to become available
+        rospy.wait_for_message('input', PoseWithCovarianceStamped)
+
+        # Subscribe to the /odom_combined topic
+        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
+        rospy.loginfo("Publishing combined odometry on /odom_ekf")
+
+    def pub_ekf_odom(self, msg):
+        odom = Odometry()
+        odom.header = msg.header
+        odom.header.frame_id = '/odom_combined'
+        odom.child_frame_id = 'base_footprint'
+        odom.pose = msg.pose
+
+        self.ekf_pub.publish(odom)
+
+if __name__ == '__main__':
+    try:
+        OdomEKF()
+        rospy.spin()
+    except:
+        pass

src/robot_bringup/launch/robot_bringup.launch

@@ -0,0 +1,45 @@
+<!--
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description:robot启动时最先需要启动的launch文件，主要加载各基础功能模块启动。
+    首先就需要启动机器人的urdf文件,这样就可以在rviz中可视化显示机器人模型.然后
+    启动下位机arduino的程序,上下位机建立连接,这样上位机就可以发送控制命令.接下来
+    启动IMU模块,开始发布姿态信息.接下来就开始进行信息融合,使用imu信息和轮式里程计
+    这样得出的里程计更为准确.在这里将启动一个语音交互系统的launch文件,该启动文件
+    将启动所有与语音交互相关的节点.
+  Author: corvin
+  History:
+    20190722:init this file.
+    20200102:增加启动imu节点和robot_pose_ekf的节点功能.
+    20200404:增加启动语音交互系统相关的节点.
+-->
+<launch>
+    <!-- (1) startup robot urdf description -->
+    <!--include file="$(find robot_description)/launch/robot_description.launch" /-->
+
+    <!-- (2) startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+    <!-- (3) startup rasp imu-6DOF board-->
+    <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
+
+    <!-- (4) startup robot_pose_ekf node -->
+    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+        <param name="output_frame" value="odom_combined"/>
+        <param name="freq" value="10.0"/>
+        <param name="sensor_timeout" value="1.2"/>
+        <param name="odom_used" value="true"/>
+        <param name="imu_used" value="true"/>
+        <param name="vo_used" value="false"/>
+        <param name="debug" value="false"/>
+        <param name="self_diagnose" value="false"/>
+    </node>
+
+    <!-- (5) /odom_combined view in rviz -->
+    <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+        <remap from="input" to="/robot_pose_ekf/odom_combined" />
+        <remap from="output" to="/odom_ekf" />
+    </node>
+
+    <!-- (6) startup voice system -->
+    <!--include file="$(find voice_system)/launch/startup_voice_system.launch" /-->
+</launch>

src/robot_bringup/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_bringup</name>
+  <version>0.0.0</version>
+  <description>The robot_bringup 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/robot_bringup</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>