更新imu模块代码,更新robot_description代码,更新robot_bringup代码

src/rasp_imu_hat_6dof/.gitignore

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

src/rasp_imu_hat_6dof/README.md

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

src/rasp_imu_hat_6dof/imu_tools/.gitignore

@@ -1,15 +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/
+.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/imu_complementary_filter/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package imu_complementary_filter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.2.2 (2020-05-25)
+------------------
+* fix install path & boost linkage issues
+* Contributors: Martin Günther, Sean Yen
+
 1.2.1 (2019-05-06)
 ------------------
 * Remove junk xml (`#93 <https://github.com/ccny-ros-pkg/imu_tools/issues/93>`_)

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/CMakeLists.txt

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

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

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

@@ -1,108 +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
+/*
+  @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/package.xml

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

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

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

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

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

@@ -2,6 +2,13 @@
 Changelog for package imu_filter_madgwick
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.2.2 (2020-05-25)
+------------------
+* Drop the signals component of Boost (`#103 <https://github.com/ccny-ros-pkg/imu_tools/issues/103>`_)
+* Add the option to remove the gravity vector (`#101 <https://github.com/ccny-ros-pkg/imu_tools/issues/101>`_)
+* fix install path & boost linkage issues
+* Contributors: Alexis Paques, Martin Günther, Mike Purvis, Sean Yen
+
 1.2.1 (2019-05-06)
 ------------------
 * Skip messages and warn if computeOrientation fails

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/COPYING

@@ -1,165 +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
+                   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/imu_filter_nodelet.xml

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

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

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

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

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

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

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

@@ -1,8 +1,8 @@
-#ifndef IMU_FILTER_MADGWICK_WORLD_FRAME_H_
-#define IMU_FILTER_MADGWICK_WORLD_FRAME_H_
-
-namespace WorldFrame {
-  enum WorldFrame { ENU, NED, NWU };
-}
-
-#endif
+#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

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

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

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

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

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

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

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

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

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

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

@@ -1,121 +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();
-}
+#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

@@ -1,117 +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));
-}
-
-
+#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

@@ -1,102 +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_ */
+
+#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

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

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

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

@@ -2,6 +2,12 @@
 Changelog for package rviz_imu_plugin
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.2.2 (2020-05-25)
+------------------
+* Export symbols so plugin can load
+* properly show/hide visualization when enabled/disabled
+* Contributors: CCNY Robotics Lab, Lou Amadio, Martin Günther, v4hn
+
 1.2.1 (2019-05-06)
 ------------------
 * Fix includes, typos and log messages

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/plugin_description.xml

@@ -1,13 +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>
+<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/src/imu_acc_visual.cpp

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

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

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

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

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

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

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

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

@@ -1,55 +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
-)
-
+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

@@ -1,72 +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.
+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/cfg/param.yaml

@@ -1,8 +1,13 @@
-##################################################
-# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+###################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:使用IIC总线来读取IMU模块的数据,并
 #   通过话题将imu数据发送出来.这里是可以配置的
-#   一些参数.
+#   一些参数.各参数意义如下:
+#   pub_data_topic:发布imu数据的话题名.
+#   pub_temp_topic:发布当前imu模块温度的话题名.
+#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
+#   link_name:imu模块的urdf中link名字.
+#   pub_hz:上述各话题发布的频率.
 # Author: corvin
 # History:
 #   20200406:init this file.

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/launch/imu_data_pub.launch

@@ -1,9 +1,9 @@
 <!---
-  Copyright:2016-2019 https://www.corvin.cn ROS小课堂
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  Description:该launch启动文件是为了启动扩展板,使其进入正常工作状态.启动后就会在
+  /imu_data话题上发布imu传感器信息.需要该信息的节点,只需订阅该话题即可.同时我们还可
+  以使用dynamic_reconfigure来动态实时的修正z轴的角度.
   Author: corvin
-  Description:该launch启动文件是为了启动扩展板，使其进入正常工作状态。启动后，就会在
-    /imu话题上发布imu传感器信息。需要该信息的节点，订阅该话题即可。同时，我们还可以使
-    用dynamic_reconfigure来动态的修正z轴的角度。
   History:
     20191031:Initial this launch file.
 -->

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/launch/imu_rviz_display.launch

@@ -1,13 +1,13 @@
 <!---
-  Copyright:2016-2019 https://www.corvin.cn ROS小课堂
+  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
+  Description:该launch启动文件是为了在启动扩展板,使其进入正常工作状态后,
+    可以在rviz中可视化查看imu扩展板当前的姿态信息.
   Author: corvin
-  Description:该launch启动文件是为了在启动扩展板，使其进入正常工作状态后。
-    可以在rviz中可视化imu扩展板当前的姿态信息。
   History:
     20191031:Initial this launch file.
 -->
 <launch>
-  <node pkg="rviz" type="rviz" name="imu_rviz_node" 
+  <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

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

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

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

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/package.xml

@@ -1,29 +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>
-
+<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/serial_imu_hat_6dof/CMakeLists.txt

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/cfg/param.yaml

@@ -1,32 +1,46 @@
-################################################
-# 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:话题发布的数据频率.
+###########################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:使用串口与IMU模块进行通信时的配置信息.
+# 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
+# 可以根据需要来修改:
+#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
+#     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
+#     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
+#     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
+#     就需要在raspi-config中关闭串口登录功能,打开硬件
+#     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
+#  baud_rate:串口通信波特率,默认为57600,如果查看发布的
+#  　　话题中没有IMU数据可以尝试换波特率.
+#  pub_data_topic:发布imu数据的ROS话题名.
+#  pub_temp_topic:发布imu模块温度的ROS话题名.
+#  link_name:imu模块的link名.
+#  pub_hz:话题发布的数据频率,默认设置为30Hz.
+#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
+#  yaw_pub_topic:将yaw信息通过该话题发送出来.
+#  pin_outHL_srv:控制IMU板上D0-D4的输出电平高低的服务.
+#  yaw_data_srv:可以获取到yaw当前角度的服务.
 # Author: corvin
 # History:
-#   20200402: init this file.
-#   20200406: 增加直接发布yaw数据的话题.
-################################################
+#   20200402:init this file.
+#   20200406:增加直接发布yaw数据的ROS话题.
+#   20210317:去掉数据位,奇偶校验,停止位参数,这几个
+#     参数模块已经固定死了,无需修改.
+#   20210318:增加yaw角度归零的话题配置参数yaw_zero_topic.
+#     增加使用服务方式将yaw归零的名称yaw_zero_service.
+#   20210319:增加修改串口波特率的配置参数baud_update_srv.
+#   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
+#     服务配置参数pin_outHL_srv.增加获取yaw数据的服务.
+###########################################################
 imu_dev: /dev/ttyS0
 baud_rate: 57600
-data_bits: 8
-parity: N
-stop_bits: 1
+pub_hz: 30
 
+link_name: base_imu_link
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
-yaw_topic: yaw_data
-link_name: base_imu_link
-pub_hz: 10
-
+yaw_pub_topic: yaw_data
+yaw_zero_topic: yaw_zero_topic
+yaw_zero_service: yaw_zero_service
+baud_update_srv: baud_update_service
+pin_outHL_srv: pin_outHL_service
+yaw_data_srv: /imu_node/GetYawData

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/include/imu_data.h

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
+  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
   Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
   Author: corvin
   History:

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/package.xml

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/GetYawData.srv

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setBaudRate.srv

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setPinOutHL.srv

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

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/srv/setYawZero.srv

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

src/robot_bringup/launch/robot_bringup.launch

@@ -8,16 +8,17 @@
   Author: corvin
   History:
     20200716:init this file.
+    20210602:使用串口获取imu模块的数据.
 -->
 <launch>
-    <!-- (1) startup robot urdf description -->
+    <!-- (1) startup panda robot urdf description -->
     <include file="$(find robot_description)/launch/robot_description.launch" />
 
     <!-- (2) startup mobilebase arduino launch -->
     <include file="$(find ros_arduino_python)/launch/arduino.launch" />
 
-    <!-- (3) startup rasp imu-6DOF board-->
-    <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
+    <!-- (3) startup rasp serial imu-6DOF board-->
+    <include file="$(find serial_imu_hat_6dof)/launch/serial_imu_hat.launch" />
 
     <!-- (4) startup rasp cam-->
     <include file="$(find rasp_camera)/launch/rasp_camera.launch" />

src/robot_description/launch/robot_description.launch

@@ -1,8 +1,10 @@
 <!--
-  Author: www.corvin.cn
+  Copyright:www.corvin.cn ROS小课堂
   Description:配置机器人各link之间关系,在rviz中显示机器人模型.
+  Author:corvin
   History:
     20190716:init this launch file.
+    20210602:将state_publisher更新为robot_state_publisher.
  -->
 <launch>
   <arg name="model" />
@@ -23,7 +25,7 @@
   <node
     name="robot_state_publisher"
     pkg="robot_state_publisher"
-    type="state_publisher" >
+    type="robot_state_publisher" >
     <param name="publish_frequency" value="20.0"/>
   </node>
 </launch>