reworked

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>
autowarefoundation · Mar 4, 2025 · 18e9a2b · 18e9a2b
1 parent 4649cf6
commit 18e9a2b
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 4 deletions.
diff --git a/...ol/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/...ol/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/...ity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml b/...ity_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/...locity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/...locity_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<double> ObstacleStopModule::calc_candidate_zero_vel_dist(

diff --git a/.../motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/.../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<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");
+    stop_margin_opposing_traffic =
+      get_or_declare_parameter<double>(node, "obstacle_stop.stop_planning.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>(