chore: cherry-pick obstacle cruise planner

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -105,6 +105,15 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   resetPathCandidate();
   resetPathReference();
 
+  // plan() should be called only when the module is in the RUNNING state, but
+  // due to planner manager implementation, it can be called in the IDLE state.
+  // TODO(Azu, Quda): consider a proper fix.
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
+    auto output = getPreviousModuleOutput();
+    path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
+    return output;
+  }
+
   auto output = module_type_->generateOutput();
   path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -523,6 +523,9 @@ void NormalLaneChange::insert_stop_point(
 
   // if input is not current lane, we can just insert the points at terminal.
   if (!is_current_lane) {
+    if (common_data_ptr_->transient_data.next_dist_buffer.min < calculation::eps) {
+      return;
+    }
     const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) -
                                          common_data_ptr_->transient_data.next_dist_buffer.min;
     set_stop_pose(arc_length_to_stop_pose, path, "next lc terminal");
@@ -749,10 +752,8 @@ lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes(
     return forward_path_length + std::max(signed_distance, 0.0);
   });
 
-  const auto backward_length = lane_change_parameters_->backward_lane_length;
-
   return route_handler->getLaneletSequence(
-    lane_change_lane.value(), getEgoPose(), backward_length, forward_length);
+    lane_change_lane.value(), getEgoPose(), 0.0, forward_length);
 }
 
 bool NormalLaneChange::hasFinishedLaneChange() const

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp

@@ -230,7 +230,8 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid)
   ASSERT_FALSE(lc_status.is_valid_path);
 }
 
-TEST_F(TestNormalLaneChange, testFilteredObjects)
+// TODO(Azu, Quda): Fix this test
+TEST_F(TestNormalLaneChange, DISABLED_testFilteredObjects)
 {
   constexpr auto is_approved = true;
   ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0);

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md

@@ -41,7 +41,7 @@ The obstacles meeting the following condition are determined as obstacles for yi
 
 - The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`.
 - The object is not crossing the ego's trajectory (\*1).
-- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle.
+- There is another object of type `obstacle_filtering.object_type.side_stopped` stopped in front of the moving obstacle.
 - The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles`
 - Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml

@@ -95,6 +95,17 @@
             bicycle: false
             pedestrian: false
 
+          # If an object does not have this label, the ego vehicle does not assume that the yielded obstacle will cut in due to the object.
+          side_stopped:
+            unknown: false
+            car: true
+            truck: true
+            bus: true
+            trailer: true
+            motorcycle: true
+            bicycle: false
+            pedestrian: false
+
         max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
 
         # if crossing vehicle is determined as target obstacles or not

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp

@@ -257,26 +257,27 @@ std::vector<CruiseObstacle> ObstacleCruiseModule::filter_cruise_obstacle_for_pre
       cruise_obstacles.push_back(*cruise_obstacle);
       continue;
     }
+  }
 
-    // 3. precise filtering for yield cruise
-    if (obstacle_filtering_param_.enable_yield) {
-      const auto yield_obstacles = find_yield_cruise_obstacles(
-        odometry, objects, predicted_objects_stamp, traj_points, vehicle_info);
-      if (yield_obstacles) {
-        for (const auto & y : yield_obstacles.value()) {
-          // Check if there is no member with the same UUID in cruise_obstacles
-          auto it = std::find_if(
-            cruise_obstacles.begin(), cruise_obstacles.end(),
-            [&y](const auto & c) { return y.uuid == c.uuid; });
-
-          // If no matching UUID found, insert yield obstacle into cruise_obstacles
-          if (it == cruise_obstacles.end()) {
-            cruise_obstacles.push_back(y);
-          }
+  // 3. precise filtering for yield cruise
+  if (obstacle_filtering_param_.enable_yield) {
+    const auto yield_obstacles = find_yield_cruise_obstacles(
+      odometry, objects, predicted_objects_stamp, traj_points, vehicle_info);
+    if (yield_obstacles) {
+      for (const auto & y : yield_obstacles.value()) {
+        // Check if there is no member with the same UUID in cruise_obstacles
+        auto it = std::find_if(
+          cruise_obstacles.begin(), cruise_obstacles.end(),
+          [&y](const auto & c) { return y.uuid == c.uuid; });
+
+        // If no matching UUID found, insert yield obstacle into cruise_obstacles
+        if (it == cruise_obstacles.end()) {
+          cruise_obstacles.push_back(y);
         }
       }
     }
   }
+
   prev_cruise_object_obstacles_ = cruise_obstacles;
 
   return cruise_obstacles;
@@ -461,14 +462,14 @@ std::optional<std::vector<CruiseObstacle>> ObstacleCruiseModule::find_yield_crui
       obstacle_filtering_param_.stopped_obstacle_velocity_threshold;
     if (is_moving) {
       const bool is_within_lat_dist_threshold =
-        o->get_dist_to_traj_lateral(traj_points) <
+        std::abs(o->get_dist_to_traj_lateral(traj_points)) <
         obstacle_filtering_param_.yield_lat_distance_threshold;
       if (is_within_lat_dist_threshold) moving_objects.push_back(o);
       return;
     }
     // lat threshold is larger for stopped obstacles
     const bool is_within_lat_dist_threshold =
-      o->get_dist_to_traj_lateral(traj_points) <
+      std::abs(o->get_dist_to_traj_lateral(traj_points)) <
       obstacle_filtering_param_.yield_lat_distance_threshold +
         obstacle_filtering_param_.max_lat_dist_between_obstacles;
     if (is_within_lat_dist_threshold) stopped_objects.push_back(o);
@@ -517,8 +518,8 @@ std::optional<std::vector<CruiseObstacle>> ObstacleCruiseModule::find_yield_crui
         longitudinal_distance_between_obstacles / moving_object_speed <
         obstacle_filtering_param_.max_obstacles_collision_time;
       if (are_obstacles_aligned && obstacles_collide_within_threshold_time) {
-        const auto yield_obstacle =
-          create_yield_cruise_obstacle(moving_object, predicted_objects_stamp, traj_points);
+        const auto yield_obstacle = create_yield_cruise_obstacle(
+          moving_object, stopped_object, predicted_objects_stamp, traj_points);
         if (yield_obstacle) {
           yield_obstacles.push_back(*yield_obstacle);
           using autoware::objects_of_interest_marker_interface::ColorName;
@@ -703,8 +704,9 @@ ObstacleCruiseModule::create_collision_points_for_outside_cruise_obstacle(
 }
 
 std::optional<CruiseObstacle> ObstacleCruiseModule::create_yield_cruise_obstacle(
-  const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
-  const std::vector<TrajectoryPoint> & traj_points)
+  const std::shared_ptr<PlannerData::Object> object,
+  const std::shared_ptr<PlannerData::Object> stopped_object,
+  const rclcpp::Time & predicted_objects_stamp, const std::vector<TrajectoryPoint> & traj_points)
 {
   if (traj_points.empty()) return std::nullopt;
   // check label
@@ -719,6 +721,15 @@ std::optional<CruiseObstacle> ObstacleCruiseModule::create_yield_cruise_obstacle
     return std::nullopt;
   }
 
+  if (!is_side_stopped_obstacle(stopped_object->predicted_object.classification.at(0).label)) {
+    RCLCPP_DEBUG(
+      logger_,
+      "[Cruise] Ignore yield obstacle (%s) since the corresponding stopped object type is not "
+      "designated as side_stopped.",
+      obj_uuid_str.substr(0, 4).c_str());
+    return std::nullopt;
+  }
+
   if (
     std::hypot(
       object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
@@ -779,6 +790,12 @@ bool ObstacleCruiseModule::is_outside_cruise_obstacle(const uint8_t label) const
   return std::find(types.begin(), types.end(), label) != types.end();
 }
 
+bool ObstacleCruiseModule::is_side_stopped_obstacle(const uint8_t label) const
+{
+  const auto & types = obstacle_filtering_param_.side_stopped_object_types;
+  return std::find(types.begin(), types.end(), label) != types.end();
+}
+
 bool ObstacleCruiseModule::is_cruise_obstacle(const uint8_t label) const
 {
   return is_inside_cruise_obstacle(label) || is_outside_cruise_obstacle(label);

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp

@@ -119,10 +119,12 @@ class ObstacleCruiseModule : public PluginModuleInterface
     const bool is_driving_forward, const VehicleInfo & vehicle_info,
     const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const;
   std::optional<CruiseObstacle> create_yield_cruise_obstacle(
-    const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
-    const std::vector<TrajectoryPoint> & traj_points);
+    const std::shared_ptr<PlannerData::Object> object,
+    const std::shared_ptr<PlannerData::Object> stopped_object,
+    const rclcpp::Time & predicted_objects_stamp, const std::vector<TrajectoryPoint> & traj_points);
   bool is_inside_cruise_obstacle(const uint8_t label) const;
   bool is_outside_cruise_obstacle(const uint8_t label) const;
+  bool is_side_stopped_obstacle(const uint8_t label) const;
   bool is_cruise_obstacle(const uint8_t label) const;
   bool is_front_collide_obstacle(
     const std::vector<TrajectoryPoint> & traj_points,

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp

@@ -64,6 +64,7 @@ struct ObstacleFilteringParam
 {
   std::vector<uint8_t> inside_object_types{};
   std::vector<uint8_t> outside_object_types{};
+  std::vector<uint8_t> side_stopped_object_types{};
   // bool use_pointcloud{};
 
   double max_lat_margin{};
@@ -93,6 +94,8 @@ struct ObstacleFilteringParam
       utils::get_target_object_type(node, "obstacle_cruise.obstacle_filtering.object_type.inside.");
     outside_object_types = utils::get_target_object_type(
       node, "obstacle_cruise.obstacle_filtering.object_type.outside.");
+    side_stopped_object_types = utils::get_target_object_type(
+      node, "obstacle_cruise.obstacle_filtering.object_type.side_stopped.");
     // use_pointcloud = get_or_declare_parameter<bool>(
     //   node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud");
 

system/autoware_default_adapi/src/planning.cpp

@@ -45,6 +45,9 @@ const std::map<std::string, std::string> conversion_map = {
   {"blind_spot", PlanningBehavior::REAR_CHECK},
   {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE},
   {"obstacle_stop_planner", PlanningBehavior::ROUTE_OBSTACLE},
+  {"obstacle_stop", PlanningBehavior::ROUTE_OBSTACLE},
+  {"obstacle_slow_down", PlanningBehavior::ROUTE_OBSTACLE},
+  {"obstacle_cruise", PlanningBehavior::ROUTE_OBSTACLE},
   {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE},
   {"walkway", PlanningBehavior::SIDEWALK},
   {"start_planner", PlanningBehavior::START_PLANNER},
@@ -217,6 +220,9 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning
     "/planning/planning_factors/no_stopping_area",
     "/planning/planning_factors/obstacle_cruise_planner",
     "/planning/planning_factors/obstacle_stop_planner",
+    "/planning/planning_factors/obstacle_stop",
+    "/planning/planning_factors/obstacle_cruise",
+    "/planning/planning_factors/obstacle_slow_down",
     "/planning/planning_factors/occlusion_spot",
     "/planning/planning_factors/run_out",
     "/planning/planning_factors/stop_line",