Skip to content
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

feat(obstacle_stop_module): maintain larger stop distance for opposing traffic #10212

Draft
wants to merge 3 commits into
base: main
Choose a base branch
from
+25 −12
Draft

feat(obstacle_stop_module): maintain larger stop distance for opposing traffic #10212

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
18e9a2b
reworked
PanConChicharron Mar 3, 2025
54776f0
final
PanConChicharron Mar 6, 2025
14e67bf
style(pre-commit): autofix
pre-commit-ci[bot] Mar 6, 2025
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
2 changes: 1 addition & 1 deletion ...ol/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin

// create initial state in the world coordinate
Eigen::VectorXd state_w = [&]() {
Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
Eigen::VectorXd state = Eigen::VectorXd::Zero(4);
const auto lateral_error_0 = x0(0);
const auto yaw_error_0 = x0(1);
state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x
Expand Down
2 changes: 2 additions & 0 deletions ...ity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@
terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m]
min_behavior_stop_margin: 3.0 # [m]

additional_stop_margin_opposing_traffic: 5.0

hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m]

Expand Down
30 changes: 19 additions & 11 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 @@ -890,15 +890,19 @@ double ObstacleStopModule::calc_desired_stop_margin(
const double dist_to_collide_on_ref_traj)
{
// calculate default stop margin
const double default_stop_margin = [&]() {
const auto ref_traj_length =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, traj_points.size() - 1);
if (dist_to_collide_on_ref_traj > ref_traj_length) {
// Use terminal margin (terminal_stop_margin) for obstacle stop
return stop_planning_param_.terminal_stop_margin;
}
return stop_planning_param_.stop_margin;
}();
const double default_stop_margin =
[&]() {
const auto ref_traj_length =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, traj_points.size() - 1);
if (dist_to_collide_on_ref_traj > ref_traj_length) {
// Use terminal margin (terminal_stop_margin) for obstacle stop
return stop_planning_param_.terminal_stop_margin;
}
return stop_planning_param_.stop_margin;
}() +
(stop_obstacle.velocity < 0.0)
? stop_planning_param_.additional_stop_margin_opposing_traffic
: 0.0;

// calculate stop margin on curve
const double stop_margin_on_curve = calc_margin_from_obstacle_on_curve(
Expand All @@ -915,10 +919,14 @@ double ObstacleStopModule::calc_desired_stop_margin(
const double stop_dist_diff =
closest_behavior_stop_dist_on_ref_traj - (dist_to_collide_on_ref_traj - stop_margin_on_curve);
if (0.0 < stop_dist_diff && stop_dist_diff < stop_margin_on_curve) {
return stop_planning_param_.min_behavior_stop_margin;
return stop_planning_param_.min_behavior_stop_margin + (stop_obstacle.velocity < 0.0)
? stop_planning_param_.additional_stop_margin_opposing_traffic
: 0.0;
}
}
return stop_margin_on_curve;
return stop_margin_on_curve + (stop_obstacle.velocity < 0.0)
? stop_planning_param_.additional_stop_margin_opposing_traffic
: 0.0;
}

std::optional<double> ObstacleStopModule::calc_candidate_zero_vel_dist(
Expand Down
3 changes: 3 additions & 0 deletions .../motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,7 @@ struct StopPlanningParam
double stop_margin{};
double terminal_stop_margin{};
double min_behavior_stop_margin{};
double additional_stop_margin_opposing_traffic{};
double hold_stop_velocity_threshold{};
double hold_stop_distance_threshold{};
bool enable_approaching_on_curve{};
Expand Down Expand Up @@ -185,6 +186,8 @@ struct StopPlanningParam
get_or_declare_parameter<double>(node, "obstacle_stop.stop_planning.terminal_stop_margin");
min_behavior_stop_margin = get_or_declare_parameter<double>(
node, "obstacle_stop.stop_planning.min_behavior_stop_margin");
additional_stop_margin_opposing_traffic = get_or_declare_parameter<double>(
node, "obstacle_stop.stop_planning.additional_stop_margin_opposing_traffic");
hold_stop_velocity_threshold = get_or_declare_parameter<double>(
node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold");
hold_stop_distance_threshold = get_or_declare_parameter<double>(
Expand Down
Loading