use motion utils function instead of shiftPose

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
autowarefoundation · Jul 9, 2024 · 614f87f · 614f87f
1 parent e24bd77
commit 614f87f
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 15 deletions.
diff --git a/...ware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/...ware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp
@@ -33,8 +33,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#include <vector>
-
 namespace autoware::motion::control::autonomous_emergency_braking::utils
 {
 using autoware::universe_utils::Polygon2d;
@@ -82,8 +80,6 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon(
  * @param obj the object
  */
 Polygon2d convertObjToPolygon(const PredictedObject & obj);
-
-Pose shiftPose(const Pose & pose, double longitudinal);
 }  // namespace autoware::motion::control::autonomous_emergency_braking::utils
 
 #endif  // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -923,8 +923,9 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker
   });
 
   if (ego_map_pose.has_value()) {
-    const double base_to_front_offset = vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m;
-    const auto ego_front_pose = utils::shiftPose(ego_map_pose.value(), base_to_front_offset);
+    const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m;
+    const auto ego_front_pose = autoware::universe_utils::calcOffsetPose(
+      ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0);
     const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker(
       ego_front_pose, "autonomous_emergency_braking", this->now(), 0);
     autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers);

diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp
@@ -146,13 +146,4 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj)
   }
   return object_polygon;
 }
-
-Pose shiftPose(const Pose & pose, double longitudinal)
-{
-  Pose shifted_pose = pose;
-  const auto yaw = tf2::getYaw(pose.orientation);
-  shifted_pose.position.x += std::cos(yaw) * longitudinal;
-  shifted_pose.position.y += std::sin(yaw) * longitudinal;
-  return shifted_pose;
-}
 }  // namespace autoware::motion::control::autonomous_emergency_braking::utils