update dist_obj_to_coll

Signed-off-by: beyza <[email protected]>
autowarefoundation · Jun 5, 2024 · f095639 · f095639
1 parent 9954717
commit f095639
Showing 1 changed file with 4 additions and 9 deletions.
diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp
@@ -53,15 +53,10 @@ std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
           auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y());
           const auto dist_ego_to_coll =
             motion_utils::calcSignedArcLength(ego_data.path.points, ego_pose.position, p);
-          const auto dist_obj_to_coll =
-            motion_utils::calcSignedArcLength(ego_data.path.points, object_pose.position, p);
-          const auto tta_cp_npc =
-            abs(dist_obj_to_coll) / object.kinematics.initial_twist_with_covariance.twist.linear.x;
-          const auto tta_cp_ego =
-            dist_ego_to_coll / ego_data.path.points[path_idx].point.longitudinal_velocity_mps;
-          if (
-            abs(dist_ego_to_coll) < closest_dist &&
-            std::abs(tta_cp_npc - tta_cp_ego) < params.time_horizon) {
+          const auto dist_obj_to_coll = tier4_autoware_utils::calcDistance2d(object_pose.position, p);
+          const auto tta_cp_npc = abs(dist_obj_to_coll) / object.kinematics.initial_twist_with_covariance.twist.linear.x;
+          const auto tta_cp_ego = dist_ego_to_coll / ego_data.path.points[path_idx].point.longitudinal_velocity_mps;
+          if (abs(dist_ego_to_coll) < closest_dist && std::abs(tta_cp_npc - tta_cp_ego) < params.time_horizon) {
             closest_dist = dist_ego_to_coll;
             closest_collision_point = p;
           }