Merge pull request #39 from ye-luo-xi-tui/imu_filter

Deprecated imu_extra_handle and add imu_filter into hardware resource layer

Author: qiayuanl

rm_common/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package rm_common
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.1.9 (2022-3-28)
+------------------
+* Add imu_filter and deprecated imu_extra_handle(Since the update frequency of the control loop is not stable, some of
+  the camera trigger signals of imu will be lost. We put the imu filter down to the hardware resource layer, so
+  imu_extra_handle is breaking. )
+* Add tof sensor interface
+* Contributors: Edwinlinks, Jie j, QiayuanLiao, yezi
+
 0.1.8 (2021-12-7)
 ------------------
 * Merge branch 'master' into master

rm_common/CMakeLists.txt

@@ -20,6 +20,8 @@ find_package(catkin REQUIRED
         geometry_msgs
         control_msgs
         controller_manager_msgs
+        imu_complementary_filter
+        imu_filter_madgwick
         realtime_tools
         dynamic_reconfigure
         )
@@ -34,6 +36,8 @@ catkin_package(
         geometry_msgs
         control_msgs
         controller_manager_msgs
+        imu_complementary_filter
+        imu_filter_madgwick
         roscpp
         dynamic_reconfigure
         DEPENDS
@@ -47,7 +51,7 @@ include_directories(
         ${EIGEN3_INCLUDE_DIR}
 )
 
-file(GLOB_RECURSE sources "src/*.cpp" "src/decision/*.cpp")
+file(GLOB_RECURSE sources "src/*.cpp" "src/decision/*.cpp" "src/filter/*.cpp")
 
 add_library(rm_common SHARED ${sources} include/rm_common/referee/data.h)
 #add_executable(test_traj test/test_traj.cpp)

rm_common/include/rm_common/filters/imu_complementary_filter.h

@@ -0,0 +1,26 @@
+//
+// Created by yezi on 2022/3/26.
+//
+
+#pragma once
+
+#include <rm_common/filters/imu_filter_base.h>
+#include <imu_complementary_filter/complementary_filter.h>
+
+namespace rm_common
+{
+class ImuComplementaryFilter : public ImuFilterBase
+{
+public:
+  ImuComplementaryFilter() = default;
+  void getOrientation(double& q0, double& q1, double& q2, double& q3) override;
+
+private:
+  void filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) override;
+  bool getFilterParam(XmlRpc::XmlRpcValue& imu_data) override;
+  // Parameters:
+  bool use_mag_;
+  // State variables:
+  imu_tools::ComplementaryFilter filter_;
+};
+}  // namespace rm_common

rm_common/include/rm_common/filters/imu_filter_base.h

@@ -0,0 +1,33 @@
+//
+// Created by yezi on 2022/3/26.
+//
+
+#pragma once
+
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/Temperature.h>
+#include <sensor_msgs/TimeReference.h>
+#include <realtime_tools/realtime_publisher.h>
+
+namespace rm_common
+{
+class ImuFilterBase
+{
+public:
+  bool init(XmlRpc::XmlRpcValue& imu_data, const std::string& name);
+  void update(ros::Time time, double* accel, double* omega, double* ori, double* accel_cov, double* omega_cov,
+              double* ori_cov, double temp, bool camera_trigger);
+  virtual void getOrientation(double& q0, double& q1, double& q2, double& q3) = 0;
+
+protected:
+  virtual bool getFilterParam(XmlRpc::XmlRpcValue& imu_data) = 0;
+  virtual void filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) = 0;
+  ros::Time last_update_;
+  bool initialized_filter_{ false };
+  std::string frame_id_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::Imu> > imu_data_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::Temperature> > imu_temp_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::TimeReference> > trigger_time_pub_;
+};
+}  // namespace rm_common

rm_common/include/rm_common/hardware_interface/imu_extra_interface.h

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>rm_common</name>
-    <version>0.1.8</version>
+    <version>0.1.9</version>
     <description>The rm_common package</description>
 
     <maintainer email="liaoqiayuan@gmail.com">qiayuan</maintainer>
@@ -16,6 +16,8 @@
     <depend>geometry_msgs</depend>
     <depend>realtime_tools</depend>
     <depend>rm_msgs</depend>
+    <depend>imu_complementary_filter</depend>
+    <depend>imu_filter_madgwick</depend>
     <depend>dynamic_reconfigure</depend>
     <depend>eigen</depend>
     <depend>control_msgs</depend>

rm_common/package.xml

@@ -0,0 +1,47 @@
+//
+// Created by yezi on 2022/3/26.
+//
+
+#include <rm_common/filters/imu_complementary_filter.h>
+
+namespace rm_common
+{
+void ImuComplementaryFilter::filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt)
+{
+  filter_.update(ax, ay, az, wx, wy, wz, dt);
+}
+void ImuComplementaryFilter::getOrientation(double& q0, double& q1, double& q2, double& q3)
+{
+  filter_.getOrientation(q0, q1, q2, q3);
+}
+bool ImuComplementaryFilter::getFilterParam(XmlRpc::XmlRpcValue& imu_data)
+{
+  double gain_acc;
+  double gain_mag;
+  bool do_bias_estimation;
+  double bias_alpha;
+  bool do_adaptive_gain;
+  auto it = imu_data.begin();
+  use_mag_ = imu_data.hasMember("use_mag") && (bool)imu_data[it->first]["use_mag"];
+  gain_acc = imu_data.hasMember("gain_acc") ? (double)imu_data[it->first]["gain_acc"] : 0.01;
+  gain_mag = imu_data.hasMember("gain_mag") ? (double)imu_data[it->first]["gain_mag"] : 0.01;
+  do_bias_estimation = !imu_data.hasMember("do_bias_estimation") || (bool)imu_data[it->first]["do_bias_estimation"];
+  bias_alpha = imu_data.hasMember("bias_alpha") ? (double)imu_data[it->first]["bias_alpha"] : 0.01;
+  do_adaptive_gain = !imu_data.hasMember("do_adaptive_gain") || (bool)imu_data[it->first]["do_adaptive_gain"];
+  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.");
+  }
+  return true;
+}
+}  // namespace rm_common

rm_common/src/filters.cpp renamed to rm_common/src/filter/filters.cpp

@@ -0,0 +1,73 @@
+//
+// Created by yezi on 2022/3/26.
+//
+
+#include <rm_common/filters/imu_filter_base.h>
+
+namespace rm_common
+{
+bool ImuFilterBase::init(XmlRpc::XmlRpcValue& imu_data, const std::string& name)
+{
+  ros::NodeHandle nh("~");
+  frame_id_ = (std::string)imu_data["frame_id"];
+  getFilterParam(imu_data);
+  imu_data_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::Imu>(nh, name + "/data", 100));
+  imu_temp_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::Temperature>(nh, name + "/temperature", 100));
+  trigger_time_pub_.reset(
+      new realtime_tools::RealtimePublisher<sensor_msgs::TimeReference>(nh, name + "/trigger_time", 100));
+  return true;
+}
+
+void ImuFilterBase::update(ros::Time time, double* accel, double* omega, double* ori, double* accel_cov,
+                           double* omega_cov, double* ori_cov, double temp, bool camera_trigger)
+{
+  if (!initialized_filter_)
+  {
+    initialized_filter_ = true;
+    last_update_ = time;
+    imu_data_pub_->msg_.header.frame_id = frame_id_;
+    return;
+  }
+
+  // Update the filter.
+  filterUpdate(accel[0], accel[1], accel[2], omega[0], omega[1], omega[2], (time - last_update_).toSec());
+  last_update_ = time;
+  getOrientation(ori[3], ori[0], ori[1], ori[2]);
+  if (camera_trigger)
+  {
+    if (imu_data_pub_->trylock())
+    {
+      imu_data_pub_->msg_.header.stamp = time;
+      imu_data_pub_->msg_.angular_velocity.x = omega[0];
+      imu_data_pub_->msg_.angular_velocity.y = omega[1];
+      imu_data_pub_->msg_.angular_velocity.z = omega[2];
+      imu_data_pub_->msg_.linear_acceleration.x = accel[0];
+      imu_data_pub_->msg_.linear_acceleration.y = accel[1];
+      imu_data_pub_->msg_.linear_acceleration.z = accel[2];
+      imu_data_pub_->msg_.orientation.x = ori[0];
+      imu_data_pub_->msg_.orientation.y = ori[1];
+      imu_data_pub_->msg_.orientation.z = ori[2];
+      imu_data_pub_->msg_.orientation.w = ori[3];
+      imu_data_pub_->msg_.orientation_covariance = { ori_cov[0], 0., 0., 0., ori_cov[4], 0., 0., 0., ori_cov[8] };
+      imu_data_pub_->msg_.angular_velocity_covariance = { omega_cov[0], 0., 0., 0.,          omega_cov[4],
+                                                          0.,           0., 0., omega_cov[8] };
+      imu_data_pub_->msg_.linear_acceleration_covariance = { accel_cov[0], 0., 0., 0.,          accel_cov[4],
+                                                             0.,           0., 0., accel_cov[8] };
+      imu_data_pub_->unlockAndPublish();
+    }
+    if (trigger_time_pub_->trylock())
+    {
+      trigger_time_pub_->msg_.header.stamp = time;
+      trigger_time_pub_->msg_.time_ref = time;
+      trigger_time_pub_->unlockAndPublish();
+    }
+    if (imu_temp_pub_->trylock())
+    {
+      imu_temp_pub_->msg_.header.stamp = time;
+      imu_temp_pub_->msg_.temperature = temp;
+      imu_temp_pub_->unlockAndPublish();
+    }
+  }
+}
+
+}  // namespace rm_common