From 18e9a2b70a487b2dc5f6c300c8bed2910ded0038 Mon Sep 17 00:00:00 2001 From: Arjun Jagdish Ram Date: Mon, 3 Mar 2025 11:56:03 +0900 Subject: [PATCH 1/3] reworked Signed-off-by: Arjun Jagdish Ram --- .../src/vehicle_model/vehicle_model_bicycle_kinematics.cpp | 2 +- .../config/obstacle_stop.param.yaml | 2 ++ .../src/obstacle_stop_module.cpp | 6 +++--- .../src/parameters.hpp | 3 +++ 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index c0052d4ff1b3e..d8ea5bbc3a486 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml index 4203a94df43a9..c90d59d659923 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml @@ -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] + 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] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index f739bed1f1d69..c6dad6dbd204b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -898,7 +898,7 @@ double ObstacleStopModule::calc_desired_stop_margin( return stop_planning_param_.terminal_stop_margin; } return stop_planning_param_.stop_margin; - }(); + }() + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.stop_margin_opposing_traffic : 0.0; // calculate stop margin on curve const double stop_margin_on_curve = calc_margin_from_obstacle_on_curve( @@ -915,10 +915,10 @@ 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_.stop_margin_opposing_traffic; } } - return stop_margin_on_curve; + return stop_margin_on_curve + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.stop_margin_opposing_traffic; } std::optional ObstacleStopModule::calc_candidate_zero_vel_dist( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp index ce910287bb77a..4e6bd26111132 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp @@ -157,6 +157,7 @@ struct StopPlanningParam double stop_margin{}; double terminal_stop_margin{}; double min_behavior_stop_margin{}; + double stop_margin_opposing_traffic{}; double hold_stop_velocity_threshold{}; double hold_stop_distance_threshold{}; bool enable_approaching_on_curve{}; @@ -185,6 +186,8 @@ struct StopPlanningParam get_or_declare_parameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); min_behavior_stop_margin = get_or_declare_parameter( node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); + stop_margin_opposing_traffic = + get_or_declare_parameter(node, "obstacle_stop.stop_planning.stop_margin_opposing_traffic"); hold_stop_velocity_threshold = get_or_declare_parameter( node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold"); hold_stop_distance_threshold = get_or_declare_parameter( From 54776f0011479aa97dc49a52ce5a7d1798cd17e9 Mon Sep 17 00:00:00 2001 From: Arjun Jagdish Ram Date: Thu, 6 Mar 2025 16:40:42 +0900 Subject: [PATCH 2/3] final --- .../config/obstacle_stop.param.yaml | 2 +- .../src/obstacle_stop_module.cpp | 6 +++--- .../src/parameters.hpp | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml index c90d59d659923..b50284dd42306 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml @@ -10,7 +10,7 @@ terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m] min_behavior_stop_margin: 3.0 # [m] - stop_margin_opposing_traffic: 5.0 + 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] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index c6dad6dbd204b..72201f2d7126d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -898,7 +898,7 @@ double ObstacleStopModule::calc_desired_stop_margin( return stop_planning_param_.terminal_stop_margin; } return stop_planning_param_.stop_margin; - }() + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.stop_margin_opposing_traffic : 0.0; + }() + (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( @@ -915,10 +915,10 @@ 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 + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.stop_margin_opposing_traffic; + 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 + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.stop_margin_opposing_traffic; + return stop_margin_on_curve + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.additional_stop_margin_opposing_traffic : 0.0; } std::optional ObstacleStopModule::calc_candidate_zero_vel_dist( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp index 4e6bd26111132..944cc875b47cb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp @@ -157,7 +157,7 @@ struct StopPlanningParam double stop_margin{}; double terminal_stop_margin{}; double min_behavior_stop_margin{}; - double stop_margin_opposing_traffic{}; + double additional_stop_margin_opposing_traffic{}; double hold_stop_velocity_threshold{}; double hold_stop_distance_threshold{}; bool enable_approaching_on_curve{}; @@ -186,8 +186,8 @@ struct StopPlanningParam get_or_declare_parameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); min_behavior_stop_margin = get_or_declare_parameter( node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); - stop_margin_opposing_traffic = - get_or_declare_parameter(node, "obstacle_stop.stop_planning.stop_margin_opposing_traffic"); + additional_stop_margin_opposing_traffic = + get_or_declare_parameter(node, "obstacle_stop.stop_planning.additional_stop_margin_opposing_traffic"); hold_stop_velocity_threshold = get_or_declare_parameter( node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold"); hold_stop_distance_threshold = get_or_declare_parameter( From 14e67bff33dada21b4465d4924d9c9eab39e5708 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 6 Mar 2025 07:44:31 +0000 Subject: [PATCH 3/3] style(pre-commit): autofix --- .../src/obstacle_stop_module.cpp | 30 ++++++++++++------- .../src/parameters.hpp | 4 +-- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index 72201f2d7126d..1fe0d22ee3eff 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -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; - }() + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.additional_stop_margin_opposing_traffic : 0.0; + 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( @@ -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 + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.additional_stop_margin_opposing_traffic : 0.0; + 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 + (stop_obstacle.velocity < 0.0) ? stop_planning_param_.additional_stop_margin_opposing_traffic : 0.0; + return stop_margin_on_curve + (stop_obstacle.velocity < 0.0) + ? stop_planning_param_.additional_stop_margin_opposing_traffic + : 0.0; } std::optional ObstacleStopModule::calc_candidate_zero_vel_dist( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp index 944cc875b47cb..ef817b9cf458f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp @@ -186,8 +186,8 @@ struct StopPlanningParam get_or_declare_parameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); min_behavior_stop_margin = get_or_declare_parameter( node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); - additional_stop_margin_opposing_traffic = - get_or_declare_parameter(node, "obstacle_stop.stop_planning.additional_stop_margin_opposing_traffic"); + additional_stop_margin_opposing_traffic = get_or_declare_parameter( + node, "obstacle_stop.stop_planning.additional_stop_margin_opposing_traffic"); hold_stop_velocity_threshold = get_or_declare_parameter( node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold"); hold_stop_distance_threshold = get_or_declare_parameter(