更新imu_tool代码,imu信息rviz中显示代码

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/imu_complementary_filter/CMakeLists.txt

@@ -1,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}

src/rasp_imu_hat_6dof/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>

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/imu_filter_madgwick/CMakeLists.txt

@@ -1,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"

src/rasp_imu_hat_6dof/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

src/rasp_imu_hat_6dof/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_;
 

src/rasp_imu_hat_6dof/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>
 

src/rasp_imu_hat_6dof/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;
+    }
+}

src/rasp_imu_hat_6dof/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_)

src/rasp_imu_hat_6dof/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;

src/rasp_imu_hat_6dof/imu_tools/imu_tools/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/CMakeLists.txt

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

src/rasp_imu_hat_6dof/imu_tools/imu_tools/package.xml

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/CHANGELOG.rst

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

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/CMakeLists.txt

@@ -1,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

src/rasp_imu_hat_6dof/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>
 

src/rasp_imu_hat_6dof/imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

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

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/imu_display.rviz

@@ -5,9 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /Status1
         - /Grid1/Offset1
       Splitter Ratio: 0.6222222447395325
-    Tree Height: 344
+    Tree Height: 774
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -27,8 +28,6 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
-Preferences:
-  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -53,11 +52,11 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Acceleration properties:
-        Acc. vector alpha: 0.800000011920929
+        Acc. vector alpha: 0.6000000238418579
         Acc. vector color: 179; 200; 255
-        Acc. vector scale: 0.05000000074505806
+        Acc. vector scale: 0.029999999329447746
         Derotate acceleration: false
-        Enable acceleration: false
+        Enable acceleration: true
       Axes properties:
         Axes scale: 1
         Enable axes: true
@@ -90,10 +89,7 @@ Visualization Manager:
     - 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
@@ -103,7 +99,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.9545544385910034
+      Distance: 1.5020098686218262
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -118,18 +114,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4303995668888092
+      Pitch: 0.22039949893951416
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.730486273765564
+      Yaw: 2.515486478805542
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: true
-  Height: 714
+  Height: 1144
   Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000021bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000440000021b000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000040000000044fc0100000002fb0000000800540069006d00650100000000000004000000028000fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000021b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003c9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000044000003c9000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064000000044fc0100000002fb0000000800540069006d00650100000000000006400000028000fffffffb0000000800540069006d0065010000000000000450000000000000000000000640000003c900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -138,6 +134,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1024
+  Width: 1600
   X: 0
-  Y: 96
+  Y: 28