fix(minimum_rule_based_planner): always shift trajectory start to ego

planning/autoware_minimum_rule_based_planner/config/minimum_rule_based_planner.param.yaml

@@ -26,7 +26,6 @@
       path_shift:
         enable: true
         minimum_shift_length: 0.5
-        minimum_shift_yaw: 0.349 # 20 deg
         minimum_shift_distance: 5.0
         min_speed_for_curvature: 2.77  # [m/s] 10 km/h
         lateral_accel_limit: 1.0      # [m/s^2]

planning/autoware_minimum_rule_based_planner/param/minimum_rule_based_planner_parameters.yaml

@@ -104,11 +104,6 @@ minimum_rule_based_planner:
         default_value: 0.1
         description: lateral offset threshold to trigger path shifting [m]
 
-      minimum_shift_yaw:
-        type: double
-        default_value: 0.1
-        description: yaw deviation threshold to trigger path shifting [rad]
-
       minimum_shift_distance:
         type: double
         default_value: 5.0

planning/autoware_minimum_rule_based_planner/schema/minimum_rule_based_planner.schema.json

@@ -115,12 +115,6 @@
                   "default": 0.5,
                   "minimum": 0.0
                 },
-                "minimum_shift_yaw": {
-                  "type": "number",
-                  "description": "Yaw deviation threshold to trigger shift [rad]",
-                  "default": 0.349,
-                  "minimum": 0.0
-                },
                 "minimum_shift_distance": {
                   "type": "number",
                   "description": "Floor for shift distance [m]",

planning/autoware_minimum_rule_based_planner/src/minimum_rule_based_planner.cpp

@@ -378,7 +378,6 @@ Trajectory MinimumRuleBasedPlannerNode::shift_trajectory_to_ego(
 
   TrajectoryShiftParams shift_params;
   shift_params.minimum_shift_length = params_.path_planning.path_shift.minimum_shift_length;
-  shift_params.minimum_shift_yaw = params_.path_planning.path_shift.minimum_shift_yaw;
   shift_params.minimum_shift_distance = params_.path_planning.path_shift.minimum_shift_distance;
   shift_params.min_speed_for_curvature = params_.path_planning.path_shift.min_speed_for_curvature;
   shift_params.lateral_accel_limit = params_.path_planning.path_shift.lateral_accel_limit;

planning/autoware_minimum_rule_based_planner/src/path_planner.cpp

@@ -1648,13 +1648,6 @@ Trajectory PathPlanner::shift_trajectory_to_ego(
   const double ego_yaw = tf2::getYaw(ego_pose.orientation);
   const double traj_yaw = tf2::getYaw(trajectory.points.at(nearest_idx).pose.orientation);
   const double signed_yaw_dev = autoware_utils::normalize_radian(ego_yaw - traj_yaw);
-  const double abs_yaw_dev = std::abs(signed_yaw_dev);
-
-  if (
-    std::abs(lateral_offset) < shift_params.minimum_shift_length &&
-    abs_yaw_dev < shift_params.minimum_shift_yaw) {
-    return trajectory;
-  }
 
   const double clamped_velocity =
     std::max(std::max(0.0, ego_velocity), shift_params.min_speed_for_curvature);

planning/autoware_minimum_rule_based_planner/src/path_planner.hpp

@@ -79,7 +79,6 @@
 struct TrajectoryShiftParams
 {
   double minimum_shift_length{0.1};      // [m] lateral offset threshold to trigger shift
-  double minimum_shift_yaw{0.1};         // [rad] yaw deviation threshold to trigger shift
   double minimum_shift_distance{5.0};    // [m] floor for shift distance
   double min_speed_for_curvature{2.77};  // [m/s] lower bound on speed for kappa0 computation
   double lateral_accel_limit{0.5};       // [m/s^2] allowed lateral acceleration budget
@@ -118,12 +117,12 @@
     double ego_velocity, const builtin_interfaces::msg::Time & stamp);
 
   // Trajectory shifting
   Trajectory shift_trajectory_to_ego(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, double ego_velocity,
     double ego_yaw_rate, const TrajectoryShiftParams & shift_params, double delta_arc_length);
 
   // Path to trajectory conversion
   Trajectory convert_path_to_trajectory(const PathWithLaneId & path, double resample_interval);
 
   // Params update
   void update_params(const Params & params);

planning/autoware_minimum_rule_based_planner/test/test_path_planner.cpp

@@ -45,7 +45,6 @@ Params make_default_params()
   params.path_planning.waypoint_group.interval_margin_ratio = 0.5;
   params.path_planning.path_shift.enable = false;
   params.path_planning.path_shift.minimum_shift_length = 0.1;
-  params.path_planning.path_shift.minimum_shift_yaw = 0.1;
   params.path_planning.path_shift.minimum_shift_distance = 5.0;
   params.path_planning.path_shift.min_speed_for_curvature = 2.77;
   params.path_planning.path_shift.lateral_accel_limit = 0.5;
@@ -121,7 +120,7 @@ TEST(PathPlannerTest, ConstructWithoutNode)
 // shift_trajectory_to_ego tests
 // ============================================================
 
-TEST(PathPlannerTest, ShiftNotNeeded)
+TEST(PathPlannerTest, ShiftAlwaysStartsAtEgo)
 {
   auto logger = rclcpp::get_logger("test_path_planner");
   auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
@@ -132,21 +131,19 @@ TEST(PathPlannerTest, ShiftNotNeeded)
   PathPlanner planner(logger, clock, make_time_keeper(), params, vehicle_info);
 
   auto traj = make_straight_trajectory(20, 1.0, 10.0f);
-  // Ego is exactly on the trajectory → no shift needed
-  auto ego_pose = make_pose(0.0, 0.0, 0.0);
+  // Keep the offset below the former shift threshold.
+  auto ego_pose = make_pose(0.0, 0.05, 0.0);
 
   TrajectoryShiftParams shift_params;
   shift_params.minimum_shift_length = 0.1;
-  shift_params.minimum_shift_yaw = 0.1;
 
   const auto result = planner.shift_trajectory_to_ego(traj, ego_pose, 10.0, 0.0, shift_params, 1.0);
 
-  // Should return the trajectory unchanged since offset and yaw are below threshold
-  ASSERT_EQ(result.points.size(), traj.points.size());
-  for (size_t i = 0; i < result.points.size(); ++i) {
-    EXPECT_NEAR(result.points[i].pose.position.x, traj.points[i].pose.position.x, 1e-3);
-    EXPECT_NEAR(result.points[i].pose.position.y, traj.points[i].pose.position.y, 1e-3);
-  }
+  ASSERT_FALSE(result.points.empty());
+  EXPECT_NEAR(result.points.front().pose.position.x, ego_pose.position.x, 1e-3);
+  EXPECT_NEAR(result.points.front().pose.position.y, ego_pose.position.y, 1e-3);
+  EXPECT_NEAR(result.points.front().pose.orientation.z, ego_pose.orientation.z, 1e-3);
+  EXPECT_NEAR(result.points.front().pose.orientation.w, ego_pose.orientation.w, 1e-3);
 }
 
 TEST(PathPlannerTest, ShiftShortTrajectory)
@@ -190,7 +187,6 @@ TEST(PathPlannerTest, ShiftNormal)
 
   TrajectoryShiftParams shift_params;
   shift_params.minimum_shift_length = 0.1;
-  shift_params.minimum_shift_yaw = 0.1;
   shift_params.minimum_shift_distance = 5.0;
   shift_params.min_speed_for_curvature = 2.77;
   shift_params.lateral_accel_limit = 0.5;