Skip to content

Commit ee1f7c6

Browse files
committed
clarify distance calculation
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent c4a0a8e commit ee1f7c6

File tree

1 file changed

+4
-3
lines changed

1 file changed

+4
-3
lines changed

planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -67,10 +67,11 @@ std::vector<autoware_auto_perception_msgs::msg::PredictedObject> filter_predicte
6767
const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4;
6868
const auto collision_distance_threshold =
6969
params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis;
70-
auto has_collision = std::abs(
70+
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_point_and_angle
71+
const auto lat_distance = std::abs(
7172
(o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) -
72-
(o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw)) <=
73-
collision_distance_threshold;
73+
(o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw));
74+
auto has_collision = lat_distance <= collision_distance_threshold;
7475
if (ego_data.earliest_stop_pose) {
7576
const auto collision_at_earliest_stop_pose =
7677
std::abs(

0 commit comments

Comments
 (0)