初始化提交

CMakeLists.txt

@@ -0,0 +1,11 @@
+# CMakeList.txt: mini6DOFIMU 的 CMake 项目，在此处包括源代码并定义
+# 项目特定的逻辑。
+#
+cmake_minimum_required (VERSION 3.8)
+
+project ("mini6DOFIMU")
+
+# 将源代码添加到此项目的可执行文件。
+add_executable (mini6DOFIMU "src/serial_imu.cpp" "src/proc_serial_data.cpp" "lib/imu_data.h" "src/serial_port.cpp" "lib/serial_port.h")
+
+# TODO: 如有需要，请添加测试并安装目标。

CMakeSettings.json

@@ -0,0 +1,15 @@
+{
+  "configurations": [
+    {
+      "name": "x64-Debug",
+      "generator": "Ninja",
+      "configurationType": "Debug",
+      "inheritEnvironments": [ "msvc_x64_x64" ],
+      "buildRoot": "${projectDir}\\out\\build\\${name}",
+      "installRoot": "${projectDir}\\out\\install\\${name}",
+      "cmakeCommandArgs": "",
+      "buildCommandArgs": "",
+      "ctestCommandArgs": ""
+    }
+  ]
+}

README.md

@@ -0,0 +1,7 @@
+### 下载代码
+```
+git clone https://code.corvin.cn:3000/corvin_zhang/mini_6dof_imu.git
+```
+
+### 编译
+打开vsstudio，导入，使用vsstudio中的cmake工具进行编译。cmake使用参考[cmake](https://docs.microsoft.com/zh-cn/cpp/build/cmake-projects-in-visual-studio?view=msvc-170&viewFallbackFrom=vs-2017)

iic_6dof_imu/CMakeLists.txt

@@ -1,199 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(iic_6dof_imu)
-
-## 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
-  rospy
-  sensor_msgs
-  std_msgs
-  message_generation
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-add_service_files(
-  FILES
-  GetYawData.srv
-)
-
-## 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
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  CATKIN_DEPENDS 
-  rospy 
-  sensor_msgs
-  std_msgs
-  message_runtime
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/iic_mode_imu.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/iic_mode_imu_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_iic_mode_imu.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

iic_6dof_imu/cfg/param.yaml

@@ -1,22 +0,0 @@
-###################################################
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用IIC总线来读取IMU模块的数据,并
-#   通过话题将imu数据发送出来.这里是可以配置的
-#   一些参数.各参数意义如下:
-#   pub_data_topic:发布imu数据的话题名.
-#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
-#   pitch_topic:发布俯仰角的话题名,单位弧度.
-#   roll_topic:发布横滚角的话题名,单位弧度.
-#   link_name:imu模块的urdf中link名字.
-#   pub_hz:上述各话题发布的频率,默认100hz.
-# Author: corvin
-# History:
-#   20211122:init this file.
-###################################################
-topic_pub_hz: 100
-pub_data_topic: imu_data
-yaw_topic: yaw_data
-pitch_topic: pitch_data
-roll_topic: roll_data
-link_name: base_imu_link
-get_yaw_service: /iic_imu_service/getYawData_service

iic_6dof_imu/launch/iic_6dof_imu.launch

@@ -1,15 +0,0 @@
-<!---
-  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
-  Description:该launch启动文件是为了使其进入正常工作状态.启动后就会在
-  /imu_data话题上发布imu传感器信息.需要该信息的节点,只需订阅该话题即可.
-  Author: corvin
-  History:
-    20211122:Initial this launch file.
--->
-<launch>
-  <arg name="iic_imu_cfg_file" default="$(find iic_6dof_imu)/cfg/param.yaml"/>
-  <node pkg="iic_6dof_imu" type="iic_6dof_imu_node.py" name="iic_6dof_imu_node" output="screen">
-      <rosparam file="$(arg iic_imu_cfg_file)" command="load"/>
-  </node>
-</launch>
-

iic_6dof_imu/package.xml

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

iic_6dof_imu/src/iic_6dof_imu_data.py

@@ -1,47 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: UTF-8 -*-
-
-# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-# Author: corvin
-# Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
-# History:
-#    20211122: Initial this file.
-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)

iic_6dof_imu/src/iic_6dof_imu_node.py

@@ -1,124 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: UTF-8 -*-
-
-# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数，
-#    然后组装成ROS中的IMU消息格式，发布到/imu_data话题中，这样有需要的
-#    节点可以直接订阅该话题即可获取到imu当前的数据.
-# Author: corvin
-# History:
-#    20211122:Initial this file.
-import rospy
-import math
-
-from std_msgs.msg import Float32
-from sensor_msgs.msg import Imu
-from iic_6dof_imu_data import MyIMU
-from iic_6dof_imu.srv import GetYawData,GetYawDataResponse
-
-degrees2rad = math.pi/180.0
-yaw = 0.0
-
-# ros server return Yaw data
-def return_yaw_data(req):
-    return GetYawDataResponse(yaw)
-
-rospy.init_node("iic_6dof_imu_node")
-
-# Get DIY config param
-topic_pub_hz    = rospy.get_param('~topic_pub_hz', '100')
-data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
-link_name       = rospy.get_param('~link_name', 'base_imu_link')
-yaw_topic_name  = rospy.get_param('~yaw_topic', 'yaw_topic')
-pitch_topic_name= rospy.get_param('~pitch_topic', 'pitch_topic')
-roll_topic_name = rospy.get_param('~roll_topic', 'roll_topic')
-get_yaw_srv     = rospy.get_param('~get_yaw_service', '/iic_imu_service/getYawData_service')
-
-#创建各话题发布者和自定义服务
-data_pub  = rospy.Publisher(data_topic_name,  Imu,     queue_size=1)
-yaw_pub   = rospy.Publisher(yaw_topic_name,   Float32, queue_size=1)
-pitch_pub = rospy.Publisher(pitch_topic_name, Float32, queue_size=1)
-roll_pub  = rospy.Publisher(roll_topic_name,  Float32, queue_size=1)
-yaw_srv   = rospy.Service(get_yaw_srv, GetYawData, return_yaw_data)
-
-#创建各话题发布的消息
-imuMsg   = Imu()
-yawMsg   = Float32()
-pitchMsg = Float32()
-rollMsg  = Float32()
-
-# Orientation covariance estimation
-imuMsg.orientation_covariance = [
-0.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
-accel_factor = 9.806  #sensor accel g convert to m/s^2.
-
-myIMU = MyIMU(0x50)
-rate = rospy.Rate(topic_pub_hz)
-
-rospy.loginfo("Now 6DOF IMU Module is working ...")
-while not rospy.is_shutdown():
-    myIMU.get_YPRAG()
-
-    #rospy.loginfo("yaw:%f pitch:%f roll:%f", myIMU.raw_yaw,
-    #              myIMU.raw_pitch, myIMU.raw_roll)
-    yaw_deg = float(myIMU.raw_yaw)
-    if yaw_deg >= 180.0:
-        yaw_deg -= 360.0
-
-    #rospy.loginfo("yaw_deg: %f", yaw_deg)
-    yaw   = yaw_deg*degrees2rad
-    pitch = float(myIMU.raw_pitch)*degrees2rad
-    roll  = float(myIMU.raw_roll)*degrees2rad
-
-    #填充话题消息,然后通过话题发布
-    yawMsg.data   = yaw
-    pitchMsg.data = pitch
-    rollMsg.data  = roll
-    yaw_pub.publish(yawMsg)
-    pitch_pub.publish(pitchMsg)
-    roll_pub.publish(rollMsg)
-
-    # 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)
-
-    rate.sleep()

iic_6dof_imu/srv/GetYawData.srv

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

imu_tools/.gitignore

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

imu_tools/.travis.yml

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

imu_tools/Dockerfile-melodic

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

imu_tools/Dockerfile-noetic

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

imu_tools/imu_complementary_filter/CHANGELOG.rst

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

imu_tools/imu_complementary_filter/CMakeLists.txt

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

imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h

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

imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h

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

imu_tools/imu_complementary_filter/launch/complementary_filter.launch

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

imu_tools/imu_complementary_filter/package.xml

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

imu_tools/imu_complementary_filter/src/complementary_filter.cpp

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

imu_tools/imu_complementary_filter/src/complementary_filter_node.cpp

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

imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp

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

imu_tools/imu_filter_madgwick/CHANGELOG.rst

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

imu_tools/imu_filter_madgwick/CMakeLists.txt

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

imu_tools/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg

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

imu_tools/imu_filter_madgwick/imu_filter_nodelet.xml

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

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

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

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_nodelet.h

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

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

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

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h

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

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h

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

imu_tools/imu_filter_madgwick/launch/imu_filter_madgwick.launch

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

imu_tools/imu_filter_madgwick/package.xml

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

imu_tools/imu_filter_madgwick/src/imu_filter.cpp

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

imu_tools/imu_filter_madgwick/src/imu_filter_node.cpp

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

imu_tools/imu_filter_madgwick/src/imu_filter_nodelet.cpp

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

imu_tools/imu_filter_madgwick/src/imu_filter_ros.cpp

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

imu_tools/imu_filter_madgwick/src/stateless_orientation.cpp

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

imu_tools/imu_filter_madgwick/test/madgwick_test.cpp

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

imu_tools/imu_filter_madgwick/test/stateless_orientation_test.cpp

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

imu_tools/imu_filter_madgwick/test/test_helpers.h

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

imu_tools/imu_tools/CHANGELOG.rst

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

imu_tools/imu_tools/CMakeLists.txt

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

imu_tools/imu_tools/package.xml

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

imu_tools/rviz_imu_plugin/CHANGELOG.rst

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

imu_tools/rviz_imu_plugin/CMakeLists.txt

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

imu_tools/rviz_imu_plugin/package.xml

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

imu_tools/rviz_imu_plugin/plugin_description.xml

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

imu_tools/rviz_imu_plugin/rosdoc.yaml

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

imu_tools/rviz_imu_plugin/src/imu_acc_visual.cpp

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

imu_tools/rviz_imu_plugin/src/imu_acc_visual.h

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

imu_tools/rviz_imu_plugin/src/imu_axes_visual.cpp

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

imu_tools/rviz_imu_plugin/src/imu_axes_visual.h

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

imu_tools/rviz_imu_plugin/src/imu_display.cpp

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

imu_tools/rviz_imu_plugin/src/imu_display.h

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

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

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

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.h

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

rviz_display_6dof_imu/CMakeLists.txt

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

rviz_display_6dof_imu/cfg/6dof_imu_display.rviz

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

rviz_display_6dof_imu/launch/rviz_display_6dof_imu.launch

@@ -1,12 +0,0 @@
-<!---
-  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
-  Description:该launch启动文件是为了可以在rviz中可视化查看imu当前的姿态信息.
-  Author: corvin
-  History:
-    20211122:Initial this launch file.
--->
-<launch>
-  <node pkg="rviz" type="rviz" name="imu_rviz_node"
-        args="-d $(find rviz_display_6dof_imu)/cfg/6dof_imu_display.rviz">
-  </node>
-</launch>

rviz_display_6dof_imu/package.xml

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

serial_6dof_imu/CMakeLists.txt

@@ -1,193 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(serial_6dof_imu)
-
-## 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
-  roscpp
-  sensor_msgs
-  std_msgs
-  message_generation
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-add_service_files(
-  FILES
-  getYawData.srv
-  setYawZero.srv
-  setIICAddr.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
-#   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
-)
-
-###########
-## 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_mode_imu.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-add_executable(serial_6dof_imu_node src/serial_imu_node.cpp src/proc_serial_data.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-add_dependencies(serial_6dof_imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(serial_6dof_imu_node
-  ${catkin_LIBRARIES}
-)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_serial_mode_imu.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

serial_6dof_imu/cfg/param.yaml

@@ -1,33 +0,0 @@
-###########################################################
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用串口与IMU模块进行通信时的配置信息,根据需要修改
-# 可以配置的各参数如下(这里需要注意参数名称和参数之间要有空格隔开):
-#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
-#     模块进行通信,都是写ttyUSB0,1,2.
-#  pub_topic_hz:话题发布的数据频率,默认设置为100Hz.
-#  imu_link_name:imu模块的link名,该名称一般在urdf模型中定义.
-#  pub_data_topic:发布imu数据的ROS话题名.
-#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
-#  yaw_pub_topic:将yaw信息通过该话题发送出来.
-#  pitch_pub_topic:将pitch数据通过该话题发布出来.
-#  roll_pub_topic:将roll数据通过该话题发布出来.
-#  yaw_zero_topic:通过向该话题发布空消息即可将yaw角度归零.
-#  yaw_zero_service:通过向该服务发送空请求即可将yaw归零.
-#  get_yaw_data_srv:可以获取到yaw当前角度的服务.
-# Author: corvin
-# History:
-#   20211122:init this file.
-###########################################################
-imu_dev: /dev/ttyUSB0
-pub_topic_hz: 100
-
-imu_link_name: base_imu_link
-pub_data_topic: imu_data
-yaw_pub_topic: yaw_data
-pitch_pub_topic: pitch_data
-roll_pub_topic: roll_data
-yaw_zero_topic: yaw_zero_topic
-
-yaw_zero_service: /serial_imu_service/setYawZero_service
-get_yaw_data_srv: /serial_imu_service/getYawData_service
-set_iic_addr_srv: /serial_imu_service/setIICAddr_service

serial_6dof_imu/include/imu_data.h

@@ -1,23 +0,0 @@
-/*******************************************************
- * Copyright:2016-2021 www.corvin.cn ROS小课堂
- * Description:使用串口操作读取imu模块的代码头文件.
- * Author: corvin
- * History:
- *   20211122:init this file.
- *******************************************************/
-#ifndef _IMU_DATA_H_
-#define _IMU_DATA_H_
-
-int initSerialPort(const char* path);
-
-int getImuData(void);
-int closeSerialPort(void);
-
-float getAcc(int flag);
-float getAngular(int flag);
-float getAngle(int flag);
-float getQuat(int flag);
-
-int makeYawZero(void);
-int updateIICAddr(std::string input);
-#endif

serial_6dof_imu/launch/serial_6dof_imu.launch

@@ -1,13 +0,0 @@
-<!--
-  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-  Description:使用串口来读取6DOF imu模块的数据,并在ros中发布使用话题发布出来.
-  Author: corvin
-  History:
-    20211122: init this file.
--->
-<launch>
-  <arg name="serial_6dof_imu_cfg" default="$(find serial_6dof_imu)/cfg/param.yaml" />
-  <node pkg="serial_6dof_imu" type="serial_6dof_imu_node" name="serial_imu_node" output="screen">
-    <rosparam file="$(arg serial_6dof_imu_cfg)" command="load" />
-  </node>
-</launch>

serial_6dof_imu/package.xml

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

serial_6dof_imu/src/proc_serial_data.cpp

@@ -1,356 +0,0 @@
-/*******************************************************************
- * Copyright:2016-2021 www.corvin.cn ROS小课堂
- * Description:使用串口方式读取和控制IMU模块信息.
- * Author: corvin
- * History:
- *   20211122:init this file.
-******************************************************************/
-#include<ros/ros.h>
-#include<stdio.h>
-#include<stdlib.h>
-#include<fcntl.h>
-#include<unistd.h>
-#include<termios.h>
-#include<string.h>
-#include<sys/time.h>
-#include<sys/types.h>
-
-#define  BYTE_CNT      55  //每次从串口中读取的字节数
-#define  ACCE_CONST    0.000488281   //用于计算加速度的常量16.0/32768.0
-#define  ANGULAR_CONST 0.061035156   //用于计算角速度的常量2000.0/32768.0
-#define  ANGLE_CONST   0.005493164   //用于计算欧拉角的常量180.0/32768.0
-
-static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
-static int port_fd = -1;  //串口打开时的文件描述符
-static float acce[3],angular[3],angle[3],quater[4];
-
-/**************************************
- * Description:关闭串口文件描述符.
- *************************************/
-int closeSerialPort(void)
-{
-    int ret = close(port_fd);
-    return ret;
-}
-
-/*****************************************************************
- * Description:向IMU模块发送解锁命令,发送完命令需要延迟10ms.
- ****************************************************************/
-static int send_unlockCmd(int fd)
-{
-    int ret = 0;
-    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
-    ret = write(fd, unLockCmd, sizeof(unLockCmd));
-    if(ret != sizeof(unLockCmd))
-    {
-        ROS_ERROR("Send IMU module unlock cmd error !");
-        return -1;
-    }
-    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
-    return 0;
-}
-
-/*****************************************************************
- * Description:向IMU模块发送保存命令,发送完命令需要延迟100ms.
- *****************************************************************/
-static int send_saveCmd(int fd)
-{
-    int ret = 0;
-    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
-    ret = write(fd, saveCmd, sizeof(saveCmd));
-    if(ret != sizeof(saveCmd))
-    {
-        ROS_ERROR("Send IMU module save cmd error !");
-        return -1;
-    }
-    ros::Duration(0.1).sleep(); //delay 100ms才能发送其他配置命令
-    return 0;
-}
-
-/*****************************************************************
- * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
- *   发送控制命令,最后就是需要发送两遍解锁命令、保存命令的操作.
- *****************************************************************/
-static int send_data(int fd, unsigned char *send_buffer, int length)
-{
-    int ret = 0;
-
-    #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
-    send_unlockCmd(fd); //发送解锁命令
-
-    //发送完控制命令,需要延时100ms
-    ret = write(fd, send_buffer, length*sizeof(unsigned char));
-    if(ret != length)
-    {
-        ROS_ERROR("Send IMU module control cmd error !");
-        return -2;
-    }
-    ros::Duration(0.1).sleep(); //delay 100ms才能发送其他配置命令
-
-    send_saveCmd(fd);  //发送保存命令
-    return 0;
-}
-
-/**************************************************************
- * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
- *   解析读取其中的44个字节,正好是4帧数据,每一帧数据都是11个字节.
- *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
- *************************************************************/
-static void parse_serialData(unsigned char chr)
-{
-    static unsigned char chrBuf[BYTE_CNT];
-    static unsigned char chrCnt = 0;  //记录读取的第几个字节
-
-    signed short sData[4]; //save 8 Byte有效信息
-    unsigned char i = 0;   //用于for循环
-    unsigned char frameSum = 0;  //存储数据帧的校验和
-
-    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
-
-    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
-    if(chrCnt < 11)
-        return;
-
-    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
-    for(i=0; i<10; i++)
-    {
-        frameSum += chrBuf[i];
-    }
-
-    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
-    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
-    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
-    {
-        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
-        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
-        return;
-    }
-
-    #if 0 //打印出完整的带帧头尾的数据帧
-    for(i=0; i<11; i++)
-      printf("0x%02x ",chrBuf[i]);
-    printf("\n");
-    #endif
-
-    memcpy(&sData[0], &chrBuf[2], 8);
-    switch(chrBuf[1])  //根据数据帧不同类型来进行解析数据
-    {
-        case 0x51: //x,y,z轴 加速度输出
-            acce[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ACCE_CONST;
-            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ACCE_CONST;
-            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ACCE_CONST;
-            break;
-        case 0x52: //角速度输出
-            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGULAR_CONST;
-            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGULAR_CONST;
-            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGULAR_CONST;
-            break;
-        case 0x53: //欧拉角度输出, roll, pitch, yaw
-            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGLE_CONST;
-            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGLE_CONST;
-            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGLE_CONST;
-            break;
-        case 0x59: //四元素输出
-            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
-            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
-            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
-            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
-            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
-            break;
-        default:
-            ROS_ERROR("parse imu data frame type error !");
-            break;
-    }
-    chrCnt = 0;
-}
-
-/********************************************************************
- * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
- *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
- *******************************************************************/
-int makeYawZero(void)
-{
-    int ret = 0;
-    unsigned char yawZeroCmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
-
-    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
-    if(ret != 0) //通过串口发送命令失败
-    {
-        ROS_ERROR("Send yaw zero control cmd error !");
-        return -1;
-    }
-    ROS_WARN("Set yaw to zero radian successfully !");
-
-    return 0;
-}
-
-/******************************************************************
- * Description:更新IMU模块的IIC地址,这里通过发送串口命令方式来更新IIC地址,
- *   完整的控制命令为:0xFF 0xAA 0x1A 0x** 0x00,更新的IIC地址为第4个字节.
- *****************************************************************/
-int updateIICAddr(std::string input)
-{
-    int ret = 0;
-    std::stringstream num;
-    int addr = 0;
-    const char *iicAddr = NULL;
-    unsigned char updateIICAddrCmd[5] = {0xFF, 0xAA, 0x1A, 0x00, 0x00};
-    
-    iicAddr = input.c_str();
-    num<<std::hex<<iicAddr+2<<std::endl;
-    num>>addr;
-    updateIICAddrCmd[3] = addr;
-
-    #if 0
-    for (int i=0; i<5; i++)
-    {
-        printf("0x%02x ", updateIICAddrCmd[i]);
-    }
-    printf("\n\n");
-    #endif
- 
-    ret = send_data(port_fd, updateIICAddrCmd, sizeof(updateIICAddrCmd));
-    if(ret != 0)
-    {
-        ROS_ERROR("Update IMU Module IIC Address Error !");
-        return -1;
-    }
-    ROS_WARN("Update IMU Module IIC Address [%s] successfully !", input.c_str());
-
-    return 0;
-}
-
-/*****************************************************************
- * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
- *   两个输入参数的意义如下:
- *   const char* path:IMU设备的挂载地址;
- ****************************************************************/
-int initSerialPort(const char* path)
-{
-    struct termios terminfo;
-    bzero(&terminfo, sizeof(terminfo));
-
-    port_fd = open(path, O_RDWR|O_NOCTTY);
-    if (-1 == port_fd)
-    {
-        ROS_ERROR("Open 6DOF IMU dev error:%s", path);
-        return -1;
-    }
-
-    //判断当前连接的设备是否为终端设备
-    if (isatty(STDIN_FILENO) == 0)
-    {
-        ROS_ERROR("IMU dev isatty error !");
-        return -2;
-    }
-
-    /*设置串口通信波特率-115200bps*/
-    cfsetispeed(&terminfo, B115200);
-    cfsetospeed(&terminfo, B115200);
-
-    //设置数据位 - 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(port_fd, TCIFLUSH);
-
-    if((tcsetattr(port_fd, TCSANOW, &terminfo)) != 0)
-    {
-        ROS_ERROR("set imu serial port attr error !");
-        return -3;
-    }
-
-    return 0;
-}
-
-/*****************************************
- * Description:得到三轴加速度信息,输入
- * 　参数可以为0,1,2分别代表x,y,z轴的
- * 　加速度信息.
- *****************************************/
-float getAcc(int flag)
-{
-    return acce[flag];
-}
-
-/******************************************
- * Description:得到角速度信息,输入参数可
- * 　以为0,1,2,分别代表x,y,z三轴的角速度
- *   信息.
- *****************************************/
-float getAngular(int flag)
-{
-    return angular[flag];
-}
-
-/********************************************
- * Description:获取yaw,pitch,roll,输入参数0,
- * 1,2可以分别获取到roll,pitch,yaw的数据.
- *******************************************/
-float getAngle(int flag)
-{
-    return angle[flag];
-}
-
-/********************************************
- * Description:输入参数0,1,2,3可以分别获取到
- * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
- * 　的imu_msg.orientation.w,x,y,z的顺序,
- * 　这里的q0是对应w参数,1,2,3对应x,y,z.
- ********************************************/
-float getQuat(int flag)
-{
-    return quater[flag];
-}
-
-/*************************************************************
- * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取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:%d byte\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;
-}

serial_6dof_imu/src/serial_imu_node.cpp

@@ -1,181 +0,0 @@
-/*****************************************************************
- * Copyright:2016-2021 www.corvin.cn ROS小课堂
- * Description:使用串口来读取IMU的数据,并通过ROS话题topic将数据发布出来.
- *   芯片中默认读取出来的加速度数据单位是g,需要将其转换为ROS中加速度规定的
- *   m/s2才能发布.
- * Author: corvin
- * History:
- *   20211122: init this file.
-*****************************************************************/
-#include <ros/ros.h>
-#include <tf/tf.h>
-#include <sensor_msgs/Imu.h>
-#include <std_msgs/Float32.h>
-#include <std_msgs/Empty.h>
-#include <imu_data.h>
-#include "serial_6dof_imu/setYawZero.h"
-#include "serial_6dof_imu/getYawData.h"
-#include "serial_6dof_imu/setIICAddr.h"
-
-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_6dof_imu::setYawZero::Request &req,
-                    serial_6dof_imu::setYawZero::Response &res)
-{
-    res.status = makeYawZero();
-    return true;
-}
-
-/**********************************************************************
- * Description:通过service服务调用方式来获取到yaw角度信息.
- **********************************************************************/
-bool getYawDataService(serial_6dof_imu::getYawData::Request &req,
-                       serial_6dof_imu::getYawData::Response &res)
-{
-    res.yaw = g_yawData;
-    return true;
-}
-
-/*******************************************************************
- * Description:通过service方式来更新IIC地址,默认地址为0x50,可以更新的地址
- *   范围为0x00 - 0x7F,注意更新的地址不能与IIC总线上其他设备地址冲突,这样才
- *   能在IIC总线上读取到所有地址上IIC设备的数据.
- ******************************************************************/
-bool setIICAddrService(serial_6dof_imu::setIICAddr::Request &req,
-                       serial_6dof_imu::setIICAddr::Response &res)
-{
-    res.status = updateIICAddr(req.address);
-    return true;
-}
-
-/********************************************************************
- * Description:入口主函数,负责通过topic和service来发布处理数据.
- *******************************************************************/
-int main(int argc, char **argv)
-{
-    float yaw, pitch, roll;
-    std::string imu_dev;
-    std::string imu_link_name;
-    std::string imu_topic_name;
-    std::string yaw_pub_topic;
-    std::string pitch_pub_topic;
-    std::string roll_pub_topic;
-    std::string yaw_zero_topic;
-    std::string yaw_zero_service;
-    std::string yaw_data_service;
-    std::string set_iic_addr_service;
-
-    int pub_topic_hz = 0;  //话题发布imu数据的频率
-    float degree2Rad = 3.1415926/180.0;
-    float acc_factor = 9.806;
-
-    ros::init(argc, argv, "imu_data_pub_node");
-    ros::NodeHandle handle;
-
-    //launch文件中加载yaml配置文件，然后从yaml配置文件中读取参数
-    ros::param::get("~imu_dev",          imu_dev);
-    ros::param::get("~imu_link_name",    imu_link_name);
-    ros::param::get("~pub_topic_hz",     pub_topic_hz);
-    ros::param::get("~pub_data_topic",   imu_topic_name);
-    ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
-    ros::param::get("~yaw_zero_service", yaw_zero_service);
-    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
-    ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
-    ros::param::get("~roll_pub_topic",   roll_pub_topic);
-    ros::param::get("~get_yaw_data_srv", yaw_data_service);
-    ros::param::get("~set_iic_addr_srv", set_iic_addr_service);
-
-    //初始化imu模块串口,根据设备号与IMU建立连接
-    int ret = initSerialPort(imu_dev.c_str());
-    if(ret < 0) //通过串口连接IMU模块失败
-    {
-        ROS_ERROR("init 6DOF IMU module serial port error !");
-        closeSerialPort();
-        return -1;
-    }
-    ROS_INFO("Now 6DOF IMU module is working...");
-
-    //定义服务,分别是yaw角度归零和获取yaw当前角度,更新IIC地址
-    ros::ServiceServer setyawSrv  = handle.advertiseService(yaw_zero_service,  yawZeroService);
-    ros::ServiceServer getYawSrv  = handle.advertiseService(yaw_data_service,  getYawDataService);
-    ros::ServiceServer setIICSrv  = handle.advertiseService(set_iic_addr_service, setIICAddrService);
-
-    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
-    ros::Publisher imu_pub   = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);
-    ros::Publisher yaw_pub   = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 2);
-    ros::Publisher pitch_pub = handle.advertise<std_msgs::Float32>(pitch_pub_topic, 2);
-    ros::Publisher roll_pub  = handle.advertise<std_msgs::Float32>(roll_pub_topic, 2);
-    ros::Rate loop_rate(pub_topic_hz);
-
-    sensor_msgs::Imu imu_msg;
-    imu_msg.header.frame_id = imu_link_name;
-    std_msgs::Float32 yaw_msg;
-    std_msgs::Float32 pitch_msg;
-    std_msgs::Float32 roll_msg;
-    while(ros::ok())
-    {
-        if(getImuData() < 0)
-            break;
-
-        imu_msg.header.stamp = ros::Time::now();
-        roll  = getAngle(0)*degree2Rad;
-        pitch = getAngle(1)*degree2Rad;
-        yaw   = getAngle(2)*degree2Rad;
-        if(yaw >= 3.1415926)
-            yaw -= 6.2831852;
-
-        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
-        yaw_msg.data   = yaw;
-        pitch_msg.data = pitch;
-        roll_msg.data  = roll;
-        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
-        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
-        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
-
-        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
-        imu_msg.orientation.x = getQuat(1);
-        imu_msg.orientation.y = getQuat(2);
-        imu_msg.orientation.z = getQuat(3);
-        imu_msg.orientation.w = getQuat(0);
-        imu_msg.orientation_covariance = {0.0025, 0, 0,
-                                          0, 0.0025, 0,
-                                          0, 0, 0.0025};
-
-        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
-        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
-        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
-        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
-                                               0, 0.02, 0,
-                                               0, 0, 0.02};
-
-        //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); //将imu数据通过话题发布出去
-        ros::spinOnce();
-        loop_rate.sleep();
-    }
-
-    closeSerialPort(); //关闭与IMU模块的串口连接
-    return 0;
-}

serial_6dof_imu/srv/getYawData.srv

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

serial_6dof_imu/srv/setIICAddr.srv

@@ -1,3 +0,0 @@
-string address
----
-int32 status

serial_6dof_imu/srv/setYawZero.srv

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

src/proc_serial_data.cpp

@@ -0,0 +1,349 @@
+/*******************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口方式读取和控制IMU模块信息.
+ * Author: corvin
+ * History:
+ *   20211122:init this file.
+******************************************************************/
+
+#include "../lib/imu_data.h"
+
+#define  BYTE_CNT      88  //每次从串口中读取的字节数
+#define  ACCE_CONST    0.000488281   //用于计算加速度的常量16.0/32768.0
+#define  ANGULAR_CONST 0.061035156   //用于计算角速度的常量2000.0/32768.0
+#define  ANGLE_CONST   0.005493164   //用于计算欧拉角的常量180.0/32768.0
+
+static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
+static int port_fd = -1; //串口打开时的文件描述符
+static float acce[3], angular[3], angle[3], quater[4];
+SerialPort serial;
+
+using namespace std;
+
+/******************************************************
+ * Description:打开IMU模块串口,输入串口号即可连接.
+ *   open()打开失败时,返回-1.
+ *   各参数意义如下:
+ *   string:IMU设备的串口地址;
+ *****************************************************/
+int openSerialPort(string imu_dev)
+{
+    int fd = 0;
+    fd = serial.open(imu_dev.c_str(), 115200, 0, 8, 1);
+   
+    if (1 != fd)
+    {
+        return fd;
+    }
+
+    return fd;
+}
+
+/**************************************
+ * Description:关闭串口文件描述符.
+ *************************************/
+void closeSerialPort(void)
+{
+    serial.close();;
+}
+
+
+/*****************************************************************
+ * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
+ *   发送控制命令,最后就是需要发送命令保存刚才的操作.解锁命令是固
+ *   定的0xFF 0xAA 0x69 0x88 0xB5,保存命令也是固定的0xFF 0xAA 0x00
+ *   0x00 0x00.
+ *****************************************************************/
+int send_data(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 = serial.send(unLockCmd, sizeof(unLockCmd));
+    if (ret != sizeof(unLockCmd))
+    {
+        cout << "Send IMU module unlock cmd error !" << endl;
+        return -1;
+    }
+    Sleep(10);
+
+#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 = serial.send(send_buffer, length * sizeof(unsigned char));
+    if (ret != length)
+    {
+        cout << "Send IMU module control cmd error !" << endl;
+        return -2;
+    }
+
+    //发送保存命令
+    ret = serial.send(saveCmd, sizeof(saveCmd));
+    if (ret != sizeof(saveCmd))
+    {
+        cout << "Send IMU module save cmd error !" << endl;
+        return -3;
+    }
+    Sleep(100);
+    return 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(yawZeroCmd, sizeof(yawZeroCmd));
+    if (ret != 0) //通过串口发送命令失败
+    {
+        cout << endl <<"发送yaw角归0命令失败！！！" << endl << endl;
+        return -1;
+    }
+    cout << endl << "yaw角归0成功 !" << endl << endl;
+
+    return 0;
+}
+
+/******************************************************************
+ * Description:更新IMU模块的IIC地址,这里通过发送串口命令方式来更新IIC地址,
+ *   完整的控制命令为:0xFF 0xAA 0x1A 0x** 0x00,更新的IIC地址为第4个字节.
+ *****************************************************************/
+int updateIICAddr(std::string input)
+{
+    int ret = 0;
+    stringstream num;
+    int addr = 0;
+    const char* iicAddr = NULL;
+    unsigned char updateIICAddrCmd[5] = { 0xFF, 0xAA, 0x1A, 0x00, 0x00 };
+
+    iicAddr = input.c_str();
+    num << std::hex << iicAddr + 2 << std::endl;
+    num >> addr;
+    updateIICAddrCmd[3] = addr;
+
+#if 0
+    for (int i = 0; i < 5; i++)
+    {
+        printf("0x%02x ", updateIICAddrCmd[i]);
+    }
+    printf("\n\n");
+#endif
+
+    ret = send_data(updateIICAddrCmd, sizeof(updateIICAddrCmd));
+    if (ret != 0)
+    {
+        cout << endl <<"更新IMU的I2C地址失败 ！！！" << endl << endl;
+        return -1;
+    }
+    cout << endl <<"更新IMU的I2C地址成功 !" << endl << endl;
+
+    return 0;
+}
+
+/********************************************************************
+ * Description:显示IMU模块获取的数据。
+ *******************************************************************/
+void show_imu_data()
+{
+    float yaw, pitch, roll;
+    float degree2Rad = 3.1415926 / 180.0;
+    float acc_factor = 9.806;
+    while (true)
+    {
+        if (getImuData() < 0)
+            break;
+        if (kbhit())
+            break;
+        roll = getAngle(0) * degree2Rad;
+        pitch = getAngle(1) * degree2Rad;
+        yaw = getAngle(2) * degree2Rad;
+        if (yaw >= 3.1415926)
+            yaw -= 6.2831852;
+
+        cout << endl;
+        cout.setf(std::ios::right);
+        cout << "roll:    " << setw(12) << roll << "  pitch:   " << setw(12) << pitch << "  yaw:     " << setw(12) << yaw << endl;
+        cout << "q_x:     " << setw(12) << getQuat(1) << "  q_y:     " << setw(12) << getQuat(2) << "  q_z:     " << setw(12) << getQuat(3) << "  q_w: " << setw(12) << getQuat(0) << endl;
+        cout << "a_v_x:   " << setw(12) << getAngular(0) * degree2Rad << "  a_v_y:   " << setw(12) << getAngular(1) * degree2Rad << "  a_v_z:   " << setw(12) << getAngular(2) * degree2Rad << endl;
+        cout << "l_acc_x: " << setw(12) << -getAcc(0) * acc_factor << "  l_acc_y: " << setw(12) << -getAcc(1) * acc_factor << "  l_acc_z: " << setw(12) << -getAcc(2) * acc_factor << endl;
+
+        Sleep(100);
+    }
+}
+
+/**************************************************************
+ * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
+ *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ *************************************************************/
+void parse_serialData(unsigned char chr)
+{
+    static unsigned char chrBuf[BYTE_CNT];
+    static unsigned char chrCnt = 0;  //记录读取的第几个字节
+
+    signed short sData[4]; //save 8 Byte有效信息
+    unsigned char i = 0;   //用于for循环
+    unsigned char frameSum = 0;  //存储数据帧的校验和
+
+    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
+
+    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
+    if (chrCnt < 11)
+        return;
+
+    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
+    for (i = 0; i < 10; i++)
+    {
+        frameSum += chrBuf[i];
+    }
+
+    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
+    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
+    if ((chrBuf[0] != 0x55) || (frameSum != chrBuf[10]))
+    {
+        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
+        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
+        return;
+    }
+
+#if 0 //打印出完整的带帧头尾的数据帧
+    for (i = 0; i < 11; i++)
+        printf("0x%02x ", chrBuf[i]);
+    printf("\n");
+#endif
+
+    memcpy(&sData[0], &chrBuf[2], 8);
+    switch (chrBuf[1])  //根据数据帧不同类型来进行解析数据
+    {
+    case 0x51: //x,y,z轴 加速度输出
+        acce[0] = ((short)(((short)chrBuf[3] << 8) | chrBuf[2])) * ACCE_CONST;
+        acce[1] = ((short)(((short)chrBuf[5] << 8) | chrBuf[4])) * ACCE_CONST;
+        acce[2] = ((short)(((short)chrBuf[7] << 8) | chrBuf[6])) * ACCE_CONST;
+        break;
+    case 0x52: //角速度输出
+        angular[0] = ((short)(((short)chrBuf[3] << 8) | chrBuf[2])) * ANGULAR_CONST;
+        angular[1] = ((short)(((short)chrBuf[5] << 8) | chrBuf[4])) * ANGULAR_CONST;
+        angular[2] = ((short)(((short)chrBuf[7] << 8) | chrBuf[6])) * ANGULAR_CONST;
+        break;
+    case 0x53: //欧拉角度输出, roll, pitch, yaw
+        angle[0] = ((short)(((short)chrBuf[3] << 8) | chrBuf[2])) * ANGLE_CONST;
+        angle[1] = ((short)(((short)chrBuf[5] << 8) | chrBuf[4])) * ANGLE_CONST;
+        angle[2] = ((short)(((short)chrBuf[7] << 8) | chrBuf[6])) * ANGLE_CONST;
+        break;
+    case 0x59: //四元素输出
+        quater[0] = ((short)(((short)chrBuf[3] << 8) | chrBuf[2])) / 32768.0;
+        quater[1] = ((short)(((short)chrBuf[5] << 8) | chrBuf[4])) / 32768.0;
+        quater[2] = ((short)(((short)chrBuf[7] << 8) | chrBuf[6])) / 32768.0;
+        quater[3] = ((short)(((short)chrBuf[9] << 8) | chrBuf[8])) / 32768.0;
+        //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
+        break;
+    default:
+        cout << "parse imu data frame type error !" << endl;
+        break;
+    }
+    chrCnt = 0;
+}
+
+
+/*****************************************************************
+ * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
+ *   两个输入参数的意义如下:
+ *   const char* path:IMU设备的挂载地址;
+ *   const int baud:IMU模块的串口通信波特率;
+ ****************************************************************/
+int initSerialPort(string imu_dev)
+{
+    port_fd = openSerialPort(imu_dev);
+    if (port_fd != 1) //打开IMU模块串口失败
+    {
+        return port_fd;
+    }
+
+    return port_fd;
+}
+
+/*****************************************
+ * 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;
+    //存储新数据前,将缓冲区清零
+    memset(r_buf, 0, sizeof(r_buf));
+    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
+    data_len = serial.receive(r_buf, sizeof(r_buf));
+    //data_len = read(port_fd, r_buf, sizeof(r_buf));
+    if (data_len <= 0) //从串口中读取数据失败
+    {
+        cout << "read serial port data failed !" << endl;
+        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/serial_imu.cpp

@@ -0,0 +1,85 @@
+/*****************************************************************
+ * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU的数据.
+ * Author: adam_zhuo
+ * History:
+ *   20211124: init this file.
+*****************************************************************/
+#include "../lib/imu_data.h"
+
+using namespace std;
+
+void show_direction() {
+    cout << "----------------------------------------" << endl;
+    cout << "----------------------------------------" << endl;
+    cout << "                                        " << endl;
+    cout << "                                        " << endl;
+    cout << "------------  0命令, 退出  -------------" << endl;
+    cout << "------  1命令，IMU模块yaw角归0  --------" << endl;
+    cout << "------  2命令，修改I2C地址      --------" << endl;
+    cout << "------  3命令，显示IMU模块数据  --------" << endl;
+    cout << "                                        " << endl;
+    cout << "                                        " << endl;
+    cout << "----------------------------------------" << endl;
+    cout << "----------------------------------------" << endl;
+}
+
+int main(int argc, char** argv)
+{
+    
+    string imu_dev = "com1";
+    int order = 0;
+    string input;
+
+    while (true)
+    {
+        cout << "输入串口号（例如：com1）：" << endl;
+        cin >> imu_dev;
+        
+        int ret = initSerialPort(imu_dev);
+        if (ret != 1) //通过串口连接IMU模块失败
+        {
+            cout << endl << "打开6自由度IMU模块串口失败 ！！！" << endl << endl;
+            closeSerialPort();
+            continue;
+        }
+        cout << endl << "打开6自由度IMU模块串口成功 !" << endl << endl;
+        break;
+    }
+
+    while (true)
+    {
+        show_direction();
+        cout << "输入指令：" << endl;
+        cin >> order;
+        if (order < 0 || order > 3)
+        {
+            cout << endl <<"输入正确的指令！！！" << endl << endl;
+            continue;
+        }
+        switch (order)
+        {
+        case 0:
+            closeSerialPort(); //关闭与IMU模块的串口连接
+            return 0;
+            break;
+        case 1:
+            makeYawZero();  //yaw角归0
+            break;
+        case 2:
+            cout << "输入I2C地址（0x00-0x7F）：" << endl;
+            cin >> input;
+            updateIICAddr(input);
+            break;
+        case 3:
+            show_imu_data();  //显示imu数据
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    closeSerialPort(); //关闭与IMU模块的串口连接
+    return 0;
+}

src/serial_port.cpp

@@ -0,0 +1,239 @@
+
+#include "../lib/serial_port.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#include <WinSock2.h>
+#include <windows.h>
+
+SerialPort::SerialPort()
+{
+
+}
+
+SerialPort::~SerialPort()
+{
+
+}
+
+bool SerialPort::open(const char* portname,
+						int baudrate,
+						char parity,
+						char databit,
+						char stopbit,
+						char synchronizeflag)
+{
+	this->synchronizeflag = synchronizeflag;
+	HANDLE hCom = NULL;
+	if (this->synchronizeflag)
+	{
+		//同步方式
+		hCom = CreateFileA(portname, //串口名
+									GENERIC_READ | GENERIC_WRITE, //支持读写
+									0, //独占方式，串口不支持共享
+									NULL,//安全属性指针，默认值为NULL
+									OPEN_EXISTING, //打开现有的串口文件
+									0, //0：同步方式，FILE_FLAG_OVERLAPPED：异步方式
+									NULL);//用于复制文件句柄，默认值为NULL，对串口而言该参数必须置为NULL
+	}
+	else
+	{
+		//异步方式
+		hCom = CreateFileA(portname, //串口名
+									GENERIC_READ | GENERIC_WRITE, //支持读写
+									0, //独占方式，串口不支持共享
+									NULL,//安全属性指针，默认值为NULL
+									OPEN_EXISTING, //打开现有的串口文件
+									FILE_FLAG_OVERLAPPED, //0：同步方式，FILE_FLAG_OVERLAPPED：异步方式
+									NULL);//用于复制文件句柄，默认值为NULL，对串口而言该参数必须置为NULL
+	}
+	
+	if(hCom == (HANDLE)-1)
+	{		
+		return false;
+	}
+
+	//配置缓冲区大小 
+	if(! SetupComm(hCom, 1024, 1024))
+	{
+		return false;
+	}
+
+	// 配置参数 
+	DCB p;
+	memset(&p, 0, sizeof(p));
+	p.DCBlength = sizeof(p);
+	p.BaudRate = baudrate; // 波特率
+	p.ByteSize = databit; // 数据位
+
+	switch (parity) //校验位
+	{   
+	case 0:   
+		p.Parity = NOPARITY; //无校验
+		break;  
+	case 1:   
+		p.Parity = ODDPARITY; //奇校验
+		break;  
+	case 2:
+		p.Parity = EVENPARITY; //偶校验
+		break;
+	case 3:
+		p.Parity = MARKPARITY; //标记校验
+		break;
+	}
+
+	switch(stopbit) //停止位
+	{
+	case 1:
+		p.StopBits = ONESTOPBIT; //1位停止位
+		break;
+	case 2:
+		p.StopBits = TWOSTOPBITS; //2位停止位
+		break;
+	case 3:
+		p.StopBits = ONE5STOPBITS; //1.5位停止位
+		break;
+	}
+
+	if(! SetCommState(hCom, &p))
+	{
+		// 设置参数失败
+		return false;
+	}
+
+	//超时处理,单位：毫秒
+	//总超时＝时间系数×读或写的字符数＋时间常量
+	COMMTIMEOUTS TimeOuts;
+	TimeOuts.ReadIntervalTimeout = 1; //读间隔超时
+	TimeOuts.ReadTotalTimeoutMultiplier = 5; //读时间系数
+	TimeOuts.ReadTotalTimeoutConstant = 5; //读时间常量
+	TimeOuts.WriteTotalTimeoutMultiplier = 500; // 写时间系数
+	TimeOuts.WriteTotalTimeoutConstant = 2000; //写时间常量
+	SetCommTimeouts(hCom,&TimeOuts);
+
+	PurgeComm(hCom,PURGE_TXCLEAR | PURGE_RXCLEAR | PURGE_TXABORT | PURGE_RXABORT);//清空串口缓冲区
+
+	memcpy(pHandle, &hCom, sizeof(hCom));// 保存句柄
+
+	return true;
+}
+
+void SerialPort::close()
+{
+	HANDLE hCom = *(HANDLE*)pHandle;
+	CloseHandle(hCom);
+}
+
+int SerialPort::send(const void *buf,int len)
+{
+	HANDLE hCom = *(HANDLE*)pHandle;
+	PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR | PURGE_TXABORT | PURGE_RXABORT);//清空串口缓冲区
+	if (this->synchronizeflag)
+	{
+		// 同步方式
+		DWORD dwBytesWrite = len; //成功写入的数据字节数
+		BOOL bWriteStat = WriteFile(hCom, //串口句柄
+									buf, //数据首地址
+									dwBytesWrite, //要发送的数据字节数
+									&dwBytesWrite, //DWORD*，用来接收返回成功发送的数据字节数
+									NULL); //NULL为同步发送，OVERLAPPED*为异步发送
+		if (!bWriteStat)
+		{
+			return 0;
+		}
+		return dwBytesWrite;
+	}
+	else
+	{
+		//异步方式
+		DWORD dwBytesWrite = len; //成功写入的数据字节数
+		DWORD dwErrorFlags; //错误标志
+		COMSTAT comStat; //通讯状态
+		OVERLAPPED m_osWrite; //异步输入输出结构体
+
+		//创建一个用于OVERLAPPED的事件处理，不会真正用到，但系统要求这么做
+		memset(&m_osWrite, 0, sizeof(m_osWrite));
+		m_osWrite.hEvent = CreateEvent(NULL, TRUE, FALSE, "WriteEvent");
+
+		ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误，获得设备当前状态
+		BOOL bWriteStat = WriteFile(hCom, //串口句柄
+			buf, //数据首地址
+			dwBytesWrite, //要发送的数据字节数
+			&dwBytesWrite, //DWORD*，用来接收返回成功发送的数据字节数
+			&m_osWrite); //NULL为同步发送，OVERLAPPED*为异步发送
+		if (!bWriteStat)
+		{
+			if (GetLastError() == ERROR_IO_PENDING) //如果串口正在写入
+			{
+				WaitForSingleObject(m_osWrite.hEvent, 1000); //等待写入事件1秒钟
+			}
+			else
+			{
+				ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误
+				CloseHandle(m_osWrite.hEvent); //关闭并释放hEvent内存
+				return 0;
+			}
+		}
+		return dwBytesWrite;
+	}
+}
+
+int SerialPort::receive(void *buf,int maxlen)
+{
+	HANDLE hCom = *(HANDLE*)pHandle;
+	PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR | PURGE_TXABORT | PURGE_RXABORT);//清空串口缓冲区
+	if (this->synchronizeflag)
+	{
+		//同步方式
+		DWORD wCount = maxlen; //成功读取的数据字节数
+		BOOL bReadStat = ReadFile(hCom, //串口句柄
+									buf, //数据首地址
+									wCount, //要读取的数据最大字节数
+									&wCount, //DWORD*,用来接收返回成功读取的数据字节数
+									NULL); //NULL为同步发送，OVERLAPPED*为异步发送
+		if (!bReadStat)
+		{
+			return 0;
+		}
+		return wCount;
+	}
+	else
+	{
+		//异步方式
+		DWORD wCount = maxlen; //成功读取的数据字节数
+		DWORD dwErrorFlags; //错误标志
+		COMSTAT comStat; //通讯状态
+		OVERLAPPED m_osRead; //异步输入输出结构体
+
+		//创建一个用于OVERLAPPED的事件处理，不会真正用到，但系统要求这么做
+		memset(&m_osRead, 0, sizeof(m_osRead));
+		m_osRead.hEvent = CreateEvent(NULL, TRUE, FALSE, "ReadEvent");
+
+		ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误，获得设备当前状态
+		if (!comStat.cbInQue)return 0; //如果输入缓冲区字节数为0，则返回false
+
+		BOOL bReadStat = ReadFile(hCom, //串口句柄
+			buf, //数据首地址
+			wCount, //要读取的数据最大字节数
+			&wCount, //DWORD*,用来接收返回成功读取的数据字节数
+			&m_osRead); //NULL为同步发送，OVERLAPPED*为异步发送
+		if (!bReadStat)
+		{
+			if (GetLastError() == ERROR_IO_PENDING) //如果串口正在读取中
+			{
+				//GetOverlappedResult函数的最后一个参数设为TRUE
+				//函数会一直等待，直到读操作完成或由于错误而返回
+				GetOverlappedResult(hCom, &m_osRead, &wCount, TRUE);
+			}
+			else
+			{
+				ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误
+				CloseHandle(m_osRead.hEvent); //关闭并释放hEvent的内存
+				return 0;
+			}
+		}
+		return wCount;
+	}
+}
+