Skip to content

fix(obstacle_stop): fix stop obstacle selection bug #43

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rej55 merged 1 commit into from
Sep 4, 2025
+11 −6
Merged

fix(obstacle_stop): fix stop obstacle selection bug #43

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 11 additions & 6 deletions ...locity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,7 @@ double calc_time_to_reach_collision_point(
}

// TODO(takagi): refactor this function as same as obstacle_filtering_param
double calc_braking_dist(
double calc_braking_dist_along_trajectory(
const StopObstacleClassification::Type label, const double lon_vel, const RSSParam & rss_params)
{
const double braking_acc = [&]() {
Expand Down Expand Up @@ -634,7 +634,7 @@ std::vector<StopObstacle> ObstacleStopModule::filter_stop_obstacle_for_point_clo
stop_candidate.vel_lpf.getValue().value(), stop_candidate.latest_collision_point.point,
time_compensated_dist_to_collide, polygon_param);
} else if (stop_planning_param_.rss_params.use_rss_stop) {
const auto braking_dist = calc_braking_dist(
const auto braking_dist = calc_braking_dist_along_trajectory(
StopObstacleClassification::Type::POINTCLOUD, *stop_candidate.vel_lpf.getValue(),
stop_planning_param_.rss_params);
stop_obstacles.emplace_back(
Expand Down Expand Up @@ -755,7 +755,7 @@ std::optional<StopObstacle> ObstacleStopModule::pick_stop_obstacle_from_predicte
}

if (stop_planning_param_.rss_params.use_rss_stop) {
const auto braking_dist = calc_braking_dist(
const auto braking_dist = calc_braking_dist_along_trajectory(
StopObstacleClassification{predicted_object.classification}.label,
object->get_lon_vel_relative_to_traj(traj_points), stop_planning_param_.rss_params);

Expand Down Expand Up @@ -929,7 +929,7 @@ std::optional<geometry_msgs::msg::Point> ObstacleStopModule::plan_stop(
};
if (
(is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj +
stop_obstacle.dist_to_collide_on_decimated_traj >
stop_obstacle.braking_dist.value_or(0.0) >
determined_stop_obstacle->dist_to_collide_on_decimated_traj +
determined_stop_obstacle->braking_dist.value_or(0.0)) ||
(!is_same_param_types &&
Expand Down Expand Up @@ -1152,8 +1152,13 @@ std::optional<geometry_msgs::msg::Point> ObstacleStopModule::calc_stop_point(
auto output_traj_points = traj_points;

// insert stop point
const auto zero_vel_idx =
autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points);
const auto zero_vel_idx = [&]() -> std::optional<size_t> {
if (determined_zero_vel_dist <= 0.0) {
return 0;
}
return autoware::motion_utils::insertStopPoint(
0, *determined_zero_vel_dist, output_traj_points);
}();
if (!zero_vel_idx) {
return std::nullopt;
}
Expand Down
Loading