fix(ekf_localizer): add imu option for roll pitch and height estimation

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

Author: meliketanrikulu

localization/ekf_localizer/config/ekf_localizer.param.yaml

@@ -49,3 +49,4 @@
       # for velocity measurement limitation (Set 0.0 if you want to ignore)
       threshold_observable_velocity_mps: 0.0 # [m/s]
       pose_frame_id: "map"
+      use_imu: false

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -33,16 +33,19 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
 #include <std_srvs/srv/set_bool.hpp>
 #include <tier4_debug_msgs/msg/float64_multi_array_stamped.hpp>
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/utils.h>
+#include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <chrono>
+#include <deque>
 #include <iostream>
 #include <memory>
 #include <queue>
@@ -129,6 +132,8 @@ class EKFLocalizer : public rclcpp::Node
   //!< @brief measurement twist with covariance subscriber
   rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
     sub_twist_with_cov_;
+  //!< @brief imu subscriber
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
   //!< @brief time for ekf calculation callback
   rclcpp::TimerBase::SharedPtr timer_control_;
   //!< @brief last predict time
@@ -154,13 +159,38 @@ class EKFLocalizer : public rclcpp::Node
 
   double ekf_dt_;
 
+  //////////////////////////////////////////////////////////////////////////////
+
   /* process noise variance for discrete model */
   double proc_cov_yaw_d_;  //!< @brief  discrete yaw process noise
   double proc_cov_vx_d_;   //!< @brief  discrete process noise in d_vx=0
   double proc_cov_wz_d_;   //!< @brief  discrete process noise in d_wz=0
 
   bool is_activated_;
 
+  double imu_dt_;
+  bool start = 0;
+  rclcpp::Time last_imu_time_ = rclcpp::Time(0, 0);
+  std::deque<sensor_msgs::msg::Imu> imu_msg_deque_;
+  sensor_msgs::msg::Imu find_closest_imu_msg(const rclcpp::Time & current_time);
+
+  builtin_interfaces::msg::Time pose_time_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  struct RollPitchHeightStruct
+  {
+    double roll;
+    double pitch;
+    double z;
+  };
+
+  RollPitchHeightStruct get_roll_pitch_height(const rclcpp::Time current_time);
+
+  rclcpp::Time prev_time_;
+  double imu_counter = 0;
+  //////////////////////////////////////////////////////////////////////////////
+
   EKFDiagnosticInfo pose_diag_info_;
   EKFDiagnosticInfo twist_diag_info_;
 
@@ -187,7 +217,10 @@ class EKFLocalizer : public rclcpp::Node
    */
   void callback_twist_with_covariance(
     geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
-
+  /**
+   * @brief get imu data
+   */
+  void callback_imu(sensor_msgs::msg::Imu::SharedPtr msg);
   /**
    * @brief set initial_pose to current EKF pose
    */

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -65,7 +65,8 @@ class HyperParameters
     warn_ellipse_size_lateral_direction(
       node->declare_parameter<double>("diagnostics.warn_ellipse_size_lateral_direction")),
     threshold_observable_velocity_mps(
-      node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
+      node->declare_parameter<double>("misc.threshold_observable_velocity_mps")),
+    use_imu(node->declare_parameter<bool>("misc.use_imu"))
   {
   }
 
@@ -100,6 +101,7 @@ class HyperParameters
   double warn_ellipse_size_lateral_direction;
 
   const double threshold_observable_velocity_mps;
+  const bool use_imu;
 };
 
 #endif  // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_

localization/ekf_localizer/schema/sub/misc.sub_schema.json

@@ -14,9 +14,14 @@
           "type": "string",
           "description": "Parent frame_id of EKF output pose",
           "default": "map"
+        },
+        "use_imu": {
+          "type": "boolean",
+          "description": "Use IMU data instead of 1D Filter for roll,pitch, position Z estimation",
+          "default": false
         }
       },
-      "required": ["threshold_observable_velocity_mps", "pose_frame_id"],
+      "required": ["threshold_observable_velocity_mps", "pose_frame_id", "use_imu"],
       "additionalProperties": false
     }
   }

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -18,13 +18,16 @@
 #include "ekf_localizer/string.hpp"
 #include "ekf_localizer/warning_message.hpp"
 #include "localization_util/covariance_ellipse.hpp"
+#include "tf2/LinearMath/Quaternion.h"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>
 #include <autoware/universe_utils/ros/msg_covariance.hpp>
 #include <rclcpp/duration.hpp>
 #include <rclcpp/logging.hpp>
 
+#include "tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
 #include <fmt/core.h>
 
 #include <algorithm>
@@ -87,6 +90,12 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
   sub_twist_with_cov_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
     "in_twist_with_covariance", 1,
     std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1));
+  if (params_.use_imu) {
+    sub_imu_ = create_subscription<sensor_msgs::msg::Imu>(
+      "/sensing/gnss/sbg/ros/imu/data", 1, std::bind(&EKFLocalizer::callback_imu, this, _1));
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  }
   service_trigger_node_ = create_service<std_srvs::srv::SetBool>(
     "trigger_node_srv",
     std::bind(
@@ -102,6 +111,8 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
   z_filter_.set_proc_dev(params_.z_filter_proc_dev);
   roll_filter_.set_proc_dev(params_.roll_filter_proc_dev);
   pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev);
+
+  std::cout << "*********************** use_imu: " << params_.use_imu << std::endl;
 }
 
 /*
@@ -230,13 +241,12 @@ void EKFLocalizer::timer_callback()
   }
   twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1);
 
-  const double z = z_filter_.get_x();
-  const double roll = roll_filter_.get_x();
-  const double pitch = pitch_filter_.get_x();
-  const geometry_msgs::msg::PoseStamped current_ekf_pose =
-    ekf_module_->get_current_pose(current_time, z, roll, pitch, false);
-  const geometry_msgs::msg::PoseStamped current_biased_ekf_pose =
-    ekf_module_->get_current_pose(current_time, z, roll, pitch, true);
+  RollPitchHeightStruct roll_pitch_height = get_roll_pitch_height(current_time);
+
+  const geometry_msgs::msg::PoseStamped current_ekf_pose = ekf_module_->get_current_pose(
+    current_time, roll_pitch_height.z, roll_pitch_height.roll, roll_pitch_height.pitch, false);
+  const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = ekf_module_->get_current_pose(
+    current_time, roll_pitch_height.z, roll_pitch_height.roll, roll_pitch_height.pitch, true);
   const geometry_msgs::msg::TwistStamped current_ekf_twist =
     ekf_module_->get_current_twist(current_time);
 
@@ -245,6 +255,53 @@ void EKFLocalizer::timer_callback()
   publish_diagnostics(current_ekf_pose, current_time);
 }
 
+/*
+ * find_closest_imu_msg
+ */
+sensor_msgs::msg::Imu EKFLocalizer::find_closest_imu_msg(const rclcpp::Time & current_time)
+{
+  sensor_msgs::msg::Imu closest_imu_msg;
+  uint64_t min_time_diff = std::numeric_limits<uint64_t>::max();
+  const int64_t pose_time_ns = current_time.nanoseconds();
+
+  for (const auto & imu_msg : imu_msg_deque_) {
+    const int64_t imu_time_ns = imu_msg.header.stamp.sec * 1e9 + imu_msg.header.stamp.nanosec;
+    uint64_t time_diff = std::abs(pose_time_ns - imu_time_ns);
+    if (time_diff < min_time_diff) {
+      min_time_diff = time_diff;
+      closest_imu_msg = imu_msg;
+    }
+  }
+  return closest_imu_msg;
+}
+
+/*
+ * get_roll_pitch_height
+ */
+EKFLocalizer::RollPitchHeightStruct EKFLocalizer::get_roll_pitch_height(
+  const rclcpp::Time current_time)
+{
+  RollPitchHeightStruct roll_pitch_height;
+
+  if (params_.use_imu && !imu_msg_deque_.empty() && imu_dt_ > 0.0) {  //???
+    sensor_msgs::msg::Imu new_imu_msg = find_closest_imu_msg(current_time);
+    roll_pitch_height.z =
+      z_filter_.get_x() + new_imu_msg.linear_acceleration.z * imu_dt_ * imu_dt_ / 2.0;
+    const auto rpy = autoware::universe_utils::getRPY(new_imu_msg.orientation);
+    roll_pitch_height.roll = rpy.x;
+    roll_pitch_height.pitch = rpy.y;
+  } else {
+    if (params_.use_imu && imu_msg_deque_.empty()) {
+      RCLCPP_WARN(this->get_logger(), "IMU data is expected but the deque is empty.");
+    }
+    roll_pitch_height.z = z_filter_.get_x();
+    roll_pitch_height.roll = roll_filter_.get_x();
+    roll_pitch_height.pitch = pitch_filter_.get_x();
+  }
+
+  return roll_pitch_height;
+}
+
 /*
  * timer_tf_callback
  */
@@ -258,15 +315,15 @@ void EKFLocalizer::timer_tf_callback()
     return;
   }
 
-  const double z = z_filter_.get_x();
-  const double roll = roll_filter_.get_x();
-  const double pitch = pitch_filter_.get_x();
-
   const rclcpp::Time current_time = this->now();
 
+  RollPitchHeightStruct roll_pitch_height = get_roll_pitch_height(current_time);
+
   geometry_msgs::msg::TransformStamped transform_stamped;
   transform_stamped = autoware::universe_utils::pose2transform(
-    ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link");
+    ekf_module_->get_current_pose(
+      current_time, roll_pitch_height.z, roll_pitch_height.roll, roll_pitch_height.pitch, false),
+    "base_link");
   transform_stamped.header.stamp = current_time;
   tf_br_->sendTransform(transform_stamped);
 }
@@ -340,6 +397,51 @@ void EKFLocalizer::callback_twist_with_covariance(
   twist_queue_.push(msg);
 }
 
+/*
+ * callback_imu
+ */
+void EKFLocalizer::callback_imu(sensor_msgs::msg::Imu::SharedPtr msg)
+{
+  sensor_msgs::msg::Imu imu_data = *msg;
+
+  geometry_msgs::msg::TransformStamped imu_to_base_link_transform;
+  try {
+    imu_to_base_link_transform =
+      tf_buffer_->lookupTransform("base_link", imu_data.header.frame_id, tf2::TimePointZero);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_WARN(this->get_logger(), "Transform Error: %s", ex.what());
+    return;
+  }
+
+  tf2::Quaternion imu_quat;
+  tf2::fromMsg(imu_data.orientation, imu_quat);
+
+  tf2::Quaternion transformed_quat = tf2::Transform(tf2::Quaternion(
+                                       imu_to_base_link_transform.transform.rotation.x,
+                                       imu_to_base_link_transform.transform.rotation.y,
+                                       imu_to_base_link_transform.transform.rotation.z,
+                                       imu_to_base_link_transform.transform.rotation.w)) *
+                                     imu_quat;
+
+  geometry_msgs::msg::Quaternion new_orientation = tf2::toMsg(transformed_quat);
+
+  sensor_msgs::msg::Imu new_imu_msg;
+  new_imu_msg = imu_data;
+  new_imu_msg.orientation = new_orientation;
+  imu_msg_deque_.push_back(new_imu_msg);
+
+  while (imu_msg_deque_.size() > 15) {
+    imu_msg_deque_.pop_front();
+  }
+  if (start == 1) {
+    imu_dt_ = std::abs((last_imu_time_ - msg->header.stamp).seconds());
+    start = 0;
+  }
+  start = 1;
+
+  last_imu_time_ = msg->header.stamp;
+}
+
 /*
  * publish_estimate_result
  */