From 614f87ff0e090029c3a03c02296a465c28259085 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 9 Jul 2024 14:25:36 +0900 Subject: [PATCH] use motion utils function instead of shiftPose Signed-off-by: Daniel Sanchez --- .../autoware/autonomous_emergency_braking/utils.hpp | 4 ---- .../autoware_autonomous_emergency_braking/src/node.cpp | 5 +++-- .../autoware_autonomous_emergency_braking/src/utils.cpp | 9 --------- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 618b1223d39cc..d2ec1763ca4dd 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -33,8 +33,6 @@ #include #endif -#include - 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 index 3382a9323dd35..69bedd02d201b 100644 --- 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 index 4b8de6f0c4428..f3b5192855d84 100644 --- 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