fix(autonomous_emergency_braking): fix imu tranform to base_link (aut…

…owarefoundation#3812)
tier4 · May 25, 2023 · 57dcf94 · 57dcf94
1 parent 2ed77fc
commit 57dcf94
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 4 deletions.
diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
@@ -24,6 +24,7 @@
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -61,6 +62,7 @@ using vehicle_info_util::VehicleInfo;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 using Path = std::vector<geometry_msgs::msg::Pose>;
+using Vector3 = geometry_msgs::msg::Vector3;
 
 struct ObjectData
 {
@@ -156,7 +158,7 @@ class AEB : public rclcpp::Node
 
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
-  Imu::ConstSharedPtr imu_ptr_{nullptr};
+  Vector3::SharedPtr angular_velocity_ptr_{nullptr};
   Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
   AutowareState::ConstSharedPtr autoware_state_{nullptr};
 

diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp
@@ -176,7 +176,24 @@ void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg)
   current_velocity_ptr_ = input_msg;
 }
 
-void AEB::onImu(const Imu::ConstSharedPtr input_msg) { imu_ptr_ = input_msg; }
+void AEB::onImu(const Imu::ConstSharedPtr input_msg)
+{
+  // transform imu
+  geometry_msgs::msg::TransformStamped transform_stamped{};
+  try {
+    transform_stamped = tf_buffer_.lookupTransform(
+      "base_link", input_msg->header.frame_id, input_msg->header.stamp,
+      rclcpp::Duration::from_seconds(0.5));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR_STREAM(
+      get_logger(),
+      "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id);
+    return;
+  }
+
+  angular_velocity_ptr_ = std::make_shared<Vector3>();
+  tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped);
+}
 
 void AEB::onPredictedTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg)
@@ -246,7 +263,7 @@ bool AEB::isDataReady()
     return missing("object pointcloud");
   }
 
-  if (use_imu_path_ && !imu_ptr_) {
+  if (use_imu_path_ && !angular_velocity_ptr_) {
     return missing("imu");
   }
 
@@ -307,7 +324,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   if (use_imu_path_) {
     Path ego_path;
     std::vector<Polygon2d> ego_polys;
-    const double current_w = imu_ptr_->angular_velocity.z;
+    const double current_w = angular_velocity_ptr_->z;
     constexpr double color_r = 0.0 / 256.0;
     constexpr double color_g = 148.0 / 256.0;
     constexpr double color_b = 205.0 / 256.0;