更新imu_tools代码

imu_tools/imu_complementary_filter/CHANGELOG.rst

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

imu_tools/imu_complementary_filter/CMakeLists.txt

@@ -1,6 +1,9 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
 project(imu_complementary_filter)
 
+find_package(Boost REQUIRED COMPONENTS thread)
+
 find_package(catkin REQUIRED COMPONENTS
   cmake_modules
   message_filters
@@ -19,6 +22,7 @@ catkin_package(
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
 )
 
 ## Declare a cpp library
@@ -28,6 +32,7 @@ add_library(complementary_filter
   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
@@ -35,7 +40,13 @@ add_executable(complementary_filter_node
   src/complementary_filter_node.cpp)
 target_link_libraries(complementary_filter_node complementary_filter ${catkin_LIBRARIES})
 
-install(TARGETS complementary_filter complementary_filter_node
+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}

imu_tools/imu_complementary_filter/package.xml

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

imu_tools/imu_filter_madgwick/CHANGELOG.rst

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

imu_tools/imu_filter_madgwick/CMakeLists.txt

@@ -1,9 +1,10 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
 project(imu_filter_madgwick)
 
 find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs tf2 tf2_geometry_msgs tf2_ros nodelet pluginlib message_filters dynamic_reconfigure)
 
-find_package(Boost REQUIRED COMPONENTS system thread signals)
+find_package(Boost REQUIRED COMPONENTS system thread)
 
 # Generate dynamic parameters
 generate_dynamic_reconfigure_options(cfg/ImuFilterMadgwick.cfg)
@@ -39,12 +40,18 @@ add_dependencies(imu_filter_node ${PROJECT_NAME}_gencfg)
 target_link_libraries(imu_filter_node imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 
-install(TARGETS imu_filter imu_filter_nodelet imu_filter_node
+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"

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

@@ -44,7 +44,7 @@ class ImuFilter
 
     // **** state variables
     double q0, q1, q2, q3;  // quaternion
-    float w_bx_, w_by_, w_bz_; // 
+    float w_bx_, w_by_, w_bz_; //
 
 public:
     void setAlgorithmGain(double gain)
@@ -99,6 +99,9 @@ public:
     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_ros.h

@@ -85,6 +85,7 @@ class ImuFilterRos
     std::string imu_frame_;
     double constant_dt_;
     bool publish_debug_topics_;
+    bool remove_gravity_vector_;
     geometry_msgs::Vector3 mag_bias_;
     double orientation_variance_;
 

imu_tools/imu_filter_madgwick/package.xml

@@ -1,14 +1,13 @@
 <package>
   <name>imu_filter_madgwick</name>
-  <version>1.2.1</version>
+  <version>1.2.2</version>
   <description>  
   Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into an orientation. Based on code by Sebastian Madgwick, http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms.
   </description>
   <license>GPL</license>
   <url>http://ros.org/wiki/imu_filter_madgwick</url>
-  <author>Ivan Dryanovski</author>
+  <author email="ivan.dryanovski@gmail.com">Ivan Dryanovski</author>
   <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
-  <maintainer email="ivan.dryanovski@gmail.com">Ivan Dryanovski</maintainer>
 
   <buildtool_depend>catkin</buildtool_depend>
 

imu_tools/imu_filter_madgwick/src/imu_filter.cpp

@@ -162,9 +162,9 @@ static inline void compensateMagneticDistortion(
 
 
 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),
-    zeta_ (0.0), gain_ (0.0), world_frame_(WorldFrame::ENU)
+    w_bx_(0.0), w_by_(0.0), w_bz_(0.0)
 {
 }
 
@@ -260,7 +260,6 @@ void ImuFilter::madgwickAHRSupdateIMU(
     float ax, float ay, float az,
     float dt)
 {
-  float recipNorm;
   float s0, s1, s2, s3;
   float qDot1, qDot2, qDot3, qDot4;
 
@@ -309,3 +308,31 @@ void ImuFilter::madgwickAHRSupdateIMU(
   // 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_ros.cpp

@@ -48,6 +48,8 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private):
    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;
 
@@ -82,6 +84,11 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private):
   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);
@@ -325,6 +332,15 @@ void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
   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_)

imu_tools/imu_filter_madgwick/src/stateless_orientation.cpp

@@ -58,7 +58,7 @@ bool StatelessOrientation::computeOrientation(
 
   float Hx, Hy, Hz;
   float Mx, My, Mz;
-  float normH, invH, invA;
+  float normH;
 
   // A: pointing up
   float Ax = A.x, Ay = A.y, Az = A.z;

imu_tools/imu_tools/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package imu_tools
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.2.2 (2020-05-25)
+------------------
+
 1.2.1 (2019-05-06)
 ------------------
 

imu_tools/imu_tools/CMakeLists.txt

@@ -1,4 +1,5 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
 project(imu_tools)
 find_package(catkin REQUIRED)
 catkin_metapackage()

imu_tools/imu_tools/package.xml

@@ -1,11 +1,10 @@
 <package>
   <name> imu_tools </name>
-  <version>1.2.1</version>
+  <version>1.2.2</version>
   <description>
     Various tools for IMU devices
   </description>
   <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
-  <maintainer email="ivan.dryanovski@gmail.com">Ivan Dryanovski</maintainer>
   <license>BSD, GPL</license>
 
   <url>http://ros.org/wiki/imu_tools</url>

imu_tools/rviz_imu_plugin/CHANGELOG.rst

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

imu_tools/rviz_imu_plugin/CMakeLists.txt

@@ -1,4 +1,5 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
 project(rviz_imu_plugin)
 
 find_package(catkin REQUIRED COMPONENTS rviz)
@@ -47,6 +48,8 @@ set(SRC_FILES  src/imu_display.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

imu_tools/rviz_imu_plugin/package.xml

@@ -1,14 +1,13 @@
 <package>
   <name>rviz_imu_plugin</name>
-  <version>1.2.1</version>
+  <version>1.2.2</version>
   <description>
     RVIZ plugin for IMU visualization
   </description>
   <license>BSD</license>
   <url>http://ros.org/wiki/rviz_imu_plugin</url>
-  <author>Ivan Dryanovski</author>
+  <author email="ivan.dryanovski@gmail.com">Ivan Dryanovski</author>
   <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
-  <maintainer email="ivan.dryanovski@gmail.com">Ivan Dryanovski</maintainer>
 
   <buildtool_depend>catkin</buildtool_depend>
 

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

@@ -174,5 +174,6 @@ inline bool ImuOrientationVisual::checkQuaternionValidity(
   return true;
 }
 
+
 } // end namespace rviz