fix(localization): add SmartPoseBuffer to handle ekf pose (#5713)

* Added smart_pose_buffer Signed-off-by: Shintaro Sakoda <[email protected]> * Added rpy_*_to_quaternion Signed-off-by: Shintaro SAKODA <[email protected]> * Fixed arg name Signed-off-by: Shintaro SAKODA <[email protected]> * Added test_smart_pose_buffer Signed-off-by: Shintaro SAKODA <[email protected]> * Fixed time jump issue in SmartPoseBuffer Signed-off-by: Shintaro SAKODA <[email protected]> * Removed pose_array_interpolator Signed-off-by: Shintaro SAKODA <[email protected]> * Removed unused functions Signed-off-by: Shintaro SAKODA <[email protected]> * Added a test about detect_time_jump_to_past Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed process of frame_id mismatch in pose callback Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed as pointed out by linter Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed mutex comment in smart_pose_buffer.hpp Signed-off-by: Shintaro SAKODA <[email protected]> * Added comment about covariance interpolation in SmartPoseBuffer Signed-off-by: Shintaro SAKODA <[email protected]> * Added const to some variables Signed-off-by: Shintaro SAKODA <[email protected]> * Fixed lock range Signed-off-by: Shintaro SAKODA <[email protected]> * Added const to some variables Signed-off-by: Shintaro SAKODA <[email protected]> * Removed a waste cast Signed-off-by: Shintaro SAKODA <[email protected]> * Fixed the code about get self pose Signed-off-by: Shintaro SAKODA <[email protected]> * Removed detect_time_jump_to_past Signed-off-by: Shintaro SAKODA <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro SAKODA <[email protected]>
autowarefoundation · Nov 30, 2023 · f89751b · f89751b
1 parent b5417a9
commit f89751b
Show file tree

Hide file tree

Showing 11 changed files with 382 additions and 360 deletions.
diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
@@ -44,7 +44,6 @@
 
 #include "ar_tag_based_localizer.hpp"
 
-#include "localization_util/pose_array_interpolator.hpp"
 #include "localization_util/util_func.hpp"
 
 #include <Eigen/Core>
@@ -94,6 +93,8 @@ bool ArTagBasedLocalizer::setup()
     RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode);
     return false;
   }
+  ekf_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
+    this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_);
 
   /*
     Log parameter info
@@ -177,24 +178,16 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
     return;
   }
 
-  // get self pose
   const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp;
-  Pose self_pose;
-  {
-    // get self-position on map
-    std::unique_lock<std::mutex> self_pose_array_lock(self_pose_array_mtx_);
-    if (self_pose_msg_ptr_array_.size() <= 1) {
-      RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!");
-      return;
-    }
-    PoseArrayInterpolator interpolator(
-      this, sensor_stamp, self_pose_msg_ptr_array_, ekf_time_tolerance_, ekf_position_tolerance_);
-    if (!interpolator.is_success()) {
-      return;
-    }
-    pop_old_pose(self_pose_msg_ptr_array_, sensor_stamp);
-    self_pose = interpolator.get_current_pose().pose.pose;
+
+  // get self pose
+  const std::optional<SmartPoseBuffer::InterpolateResult> interpolate_result =
+    ekf_pose_buffer_->interpolate(sensor_stamp);
+  if (!interpolate_result) {
+    return;
   }
+  ekf_pose_buffer_->pop_old(sensor_stamp);
+  const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose;
 
   // detect
   const std::vector<landmark_manager::Landmark> landmarks = detect_landmarks(msg);
@@ -305,37 +298,14 @@ void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & m
 
 void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg)
 {
-  // lock mutex for initial pose
-  std::lock_guard<std::mutex> self_pose_array_lock(self_pose_array_mtx_);
-  // if rosbag restart, clear buffer
-  if (!self_pose_msg_ptr_array_.empty()) {
-    const builtin_interfaces::msg::Time & t_front = self_pose_msg_ptr_array_.front()->header.stamp;
-    const builtin_interfaces::msg::Time & t_msg = msg->header.stamp;
-    if (t_front.sec > t_msg.sec || (t_front.sec == t_msg.sec && t_front.nanosec > t_msg.nanosec)) {
-      self_pose_msg_ptr_array_.clear();
-    }
-  }
-
   if (msg->header.frame_id == "map") {
-    self_pose_msg_ptr_array_.push_back(msg);
+    ekf_pose_buffer_->push_back(msg);
   } else {
-    TransformStamped transform_self_pose_frame_to_map;
-    try {
-      transform_self_pose_frame_to_map = tf_buffer_->lookupTransform(
-        "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1));
-
-      // transform self_pose_frame to map_frame
-      auto self_pose_on_map_ptr = std::make_shared<PoseWithCovarianceStamped>();
-      self_pose_on_map_ptr->pose.pose =
-        tier4_autoware_utils::transformPose(msg->pose.pose, transform_self_pose_frame_to_map);
-      // self_pose_on_map_ptr->pose.covariance;  // TODO(YamatoAndo)
-      self_pose_on_map_ptr->header.stamp = msg->header.stamp;
-      self_pose_msg_ptr_array_.push_back(self_pose_on_map_ptr);
-    } catch (tf2::TransformException & ex) {
-      RCLCPP_WARN(
-        get_logger(), "cannot get map to %s transform. %s", msg->header.frame_id.c_str(),
-        ex.what());
-    }
+    RCLCPP_ERROR_STREAM_THROTTLE(
+      get_logger(), *this->get_clock(), 1000,
+      "Received initial pose message with frame_id "
+        << msg->header.frame_id << ", but expected map. "
+        << "Please check the frame_id in the input topic and ensure it is correct.");
   }
 }
 

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
@@ -46,6 +46,7 @@
 #define AR_TAG_BASED_LOCALIZER_HPP_
 
 #include "landmark_manager/landmark_manager.hpp"
+#include "localization_util/smart_pose_buffer.hpp"
 
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -124,8 +125,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   aruco::MarkerDetector detector_;
   aruco::CameraParameters cam_param_;
   bool cam_info_received_;
-  std::mutex self_pose_array_mtx_;
-  std::deque<PoseWithCovarianceStamped::ConstSharedPtr> self_pose_msg_ptr_array_;
+  std::unique_ptr<SmartPoseBuffer> ekf_pose_buffer_;
   std::map<std::string, Pose> landmark_map_;
 };
 

diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt
@@ -6,10 +6,18 @@ autoware_package()
 
 ament_auto_add_library(localization_util SHARED
   src/util_func.cpp
-  src/pose_array_interpolator.cpp
   src/tf2_listener_module.cpp
+  src/smart_pose_buffer.cpp
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_smart_pose_buffer
+    test/test_smart_pose_buffer.cpp
+    src/smart_pose_buffer.cpp
+  )
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
 )
diff --git a/...lization_util/pose_array_interpolator.hpp → ...e/localization_util/smart_pose_buffer.hpp b/...lization_util/pose_array_interpolator.hpp → ...e/localization_util/smart_pose_buffer.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_
-#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_
+#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
 
 #include "localization_util/util_func.hpp"
 
@@ -23,33 +23,39 @@
 
 #include <deque>
 
-class PoseArrayInterpolator
+class SmartPoseBuffer
 {
 private:
   using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
 
 public:
-  PoseArrayInterpolator(
-    rclcpp::Node * node, const rclcpp::Time & target_ros_time,
-    const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array,
-    const double & pose_timeout_sec, const double & pose_distance_tolerance_meters);
+  struct InterpolateResult
+  {
+    PoseWithCovarianceStamped old_pose;
+    PoseWithCovarianceStamped new_pose;
+    PoseWithCovarianceStamped interpolated_pose;
+  };
 
-  PoseArrayInterpolator(
-    rclcpp::Node * node, const rclcpp::Time & target_ros_time,
-    const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array);
+  SmartPoseBuffer() = delete;
+  SmartPoseBuffer(
+    const rclcpp::Logger & logger, const double & pose_timeout_sec,
+    const double & pose_distance_tolerance_meters);
 
-  PoseWithCovarianceStamped get_current_pose();
-  PoseWithCovarianceStamped get_old_pose();
-  PoseWithCovarianceStamped get_new_pose();
-  [[nodiscard]] bool is_success() const;
+  std::optional<InterpolateResult> interpolate(const rclcpp::Time & target_ros_time);
+
+  void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr);
+
+  void pop_old(const rclcpp::Time & target_ros_time);
+
+  void clear();
 
 private:
   rclcpp::Logger logger_;
-  rclcpp::Clock clock_;
-  const PoseWithCovarianceStamped::SharedPtr current_pose_ptr_;
-  PoseWithCovarianceStamped::SharedPtr old_pose_ptr_;
-  PoseWithCovarianceStamped::SharedPtr new_pose_ptr_;
-  bool success_;
+  std::deque<PoseWithCovarianceStamped::ConstSharedPtr> pose_buffer_;
+  std::mutex mutex_;  // This mutex is for pose_buffer_
+
+  const double pose_timeout_sec_;
+  const double pose_distance_tolerance_meters_;
 
   [[nodiscard]] bool validate_time_stamp_difference(
     const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
@@ -59,4 +65,4 @@ class PoseArrayInterpolator
     const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
 };
 
-#endif  // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_
+#endif  // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp
@@ -46,16 +46,14 @@ geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
 geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
 geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
 
+geometry_msgs::msg::Quaternion rpy_rad_to_quaternion(
+  const double r_rad, const double p_rad, const double y_rad);
+geometry_msgs::msg::Quaternion rpy_deg_to_quaternion(
+  const double r_deg, const double p_deg, const double y_deg);
+
 geometry_msgs::msg::Twist calc_twist(
   const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
 
-void get_nearest_timestamp_pose(
-  const std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr> &
-    pose_cov_msg_ptr_array,
-  const rclcpp::Time & time_stamp,
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr,
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr);
-
 geometry_msgs::msg::PoseStamped interpolate_pose(
   const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
   const rclcpp::Time & time_stamp);
@@ -64,19 +62,11 @@ geometry_msgs::msg::PoseStamped interpolate_pose(
   const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
   const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
 
-void pop_old_pose(
-  std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr> &
-    pose_cov_msg_ptr_array,
-  const rclcpp::Time & time_stamp);
-
 Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
 Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
 geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
 Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
 
-std::vector<geometry_msgs::msg::Pose> create_random_pose_array(
-  const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num);
-
 template <class T>
 T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform)
 {
@@ -88,7 +78,7 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf
 double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
 
 void output_pose_with_cov_to_log(
-  const rclcpp::Logger logger, const std::string & prefix,
+  const rclcpp::Logger & logger, const std::string & prefix,
   const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
 
 #endif  // LOCALIZATION_UTIL__UTIL_FUNC_HPP_
diff --git a/localization/localization_util/src/pose_array_interpolator.cpp b/localization/localization_util/src/pose_array_interpolator.cpp