File tree 1 file changed +9
-4
lines changed
planning/behavior_velocity_dynamic_obstacle_stop_module/src
1 file changed +9
-4
lines changed Original file line number Diff line number Diff line change @@ -53,10 +53,15 @@ std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
53
53
auto p = geometry_msgs::msg::Point ().set__x (coll_p.x ()).set__y (coll_p.y ());
54
54
const auto dist_ego_to_coll =
55
55
motion_utils::calcSignedArcLength (ego_data.path .points , ego_pose.position , p);
56
- const auto dist_obj_to_coll = tier4_autoware_utils::calcDistance2d (object_pose.position , p);
57
- const auto tta_cp_npc = abs (dist_obj_to_coll) / object.kinematics .initial_twist_with_covariance .twist .linear .x ;
58
- const auto tta_cp_ego = dist_ego_to_coll / ego_data.path .points [path_idx].point .longitudinal_velocity_mps ;
59
- if (abs (dist_ego_to_coll) < closest_dist && std::abs (tta_cp_npc - tta_cp_ego) < params.time_horizon ) {
56
+ const auto dist_obj_to_coll =
57
+ tier4_autoware_utils::calcDistance2d (object_pose.position , p);
58
+ const auto tta_cp_npc =
59
+ abs (dist_obj_to_coll) / object.kinematics .initial_twist_with_covariance .twist .linear .x ;
60
+ const auto tta_cp_ego =
61
+ dist_ego_to_coll / ego_data.path .points [path_idx].point .longitudinal_velocity_mps ;
62
+ if (
63
+ abs (dist_ego_to_coll) < closest_dist &&
64
+ std::abs (tta_cp_npc - tta_cp_ego) < params.time_horizon ) {
60
65
closest_dist = dist_ego_to_coll;
61
66
closest_collision_point = p;
62
67
}
You can’t perform that action at this time.
0 commit comments