feat(obstacle_cruise_planner): yield function for ocp (#6242)

* WIP make ego yield to cut in vehicles

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* param name change

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add params

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* refactor

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* fix bug for distance comparison

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* implement a collision time check for yield candidates

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* deleting comments

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* comment out prints for test

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete comments, use param

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* Add param to set moving obstacle speed threshold

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* WIP check for cars going opposite direction

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* check opposite line driving

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* update documentation

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* update README

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* set default false

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* refactor

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* refactor isFrontObstacle

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* make const

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

Author: danielsanchezaran

planning/obstacle_cruise_planner/README.md

@@ -108,6 +108,20 @@ The obstacles meeting the following condition are determined as obstacles for cr
 | `behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold`         | double | maximum obstacle velocity for cruise obstacle outside the trajectory |
 | `behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold` | double | maximum overlap time of the collision between the ego and obstacle   |
 
+##### Yield for vehicles that might cut in into the ego's lane
+
+It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane.
+
+The obstacles meeting the following condition are determined as obstacles for yielding (cruising).
+
+- The object type is for cruising according to `common.cruise_obstacle_type.*` and it is moving with a speed greater than `behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold`.
+- The object is not crossing the ego's trajectory (\*1).
+- There is another object of type `common.cruise_obstacle_type.*` stopped in front of the moving obstacle.
+- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `behavior_determination.cruise.yield.max_lat_dist_between_obstacles`
+- Both obstacles, moving and stopped, are within `behavior_determination.cruise.yield.lat_distance_threshold` and `behavior_determination.cruise.yield.lat_distance_threshold` + `behavior_determination.cruise.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
+
+If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle.
+
 #### Determine stop vehicles
 
 Among obstacles which are not for cruising, the obstacles meeting the following condition are determined as obstacles for stopping.

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -95,7 +95,12 @@
           obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
           ego_obstacle_overlap_time_threshold : 2.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
           max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
-
+        yield:
+          enable_yield: false
+          lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
+          max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
+          max_obstacles_collision_time: 10.0 # how far the blocking obstacle
+          stopped_obstacle_velocity_threshold: 0.5
       slow_down:
         max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
         lat_hysteresis_margin: 0.2

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -55,15 +55,18 @@ struct Obstacle
 {
   Obstacle(
     const rclcpp::Time & arg_stamp, const PredictedObject & object,
-    const geometry_msgs::msg::Pose & arg_pose)
+    const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance,
+    const double lat_dist_from_obstacle_to_traj)
   : stamp(arg_stamp),
     pose(arg_pose),
     orientation_reliable(true),
     twist(object.kinematics.initial_twist_with_covariance.twist),
     twist_reliable(true),
     classification(object.classification.at(0)),
     uuid(tier4_autoware_utils::toHexString(object.object_id)),
-    shape(object.shape)
+    shape(object.shape),
+    ego_to_obstacle_distance(ego_to_obstacle_distance),
+    lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj)
   {
     predicted_paths.clear();
     for (const auto & path : object.kinematics.predicted_paths) {
@@ -82,6 +85,8 @@ struct Obstacle
   std::string uuid;
   Shape shape;
   std::vector<PredictedPath> predicted_paths;
+  double ego_to_obstacle_distance;
+  double lat_dist_from_obstacle_to_traj;
 };
 
 struct TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -70,6 +70,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   std::optional<geometry_msgs::msg::Point> createCollisionPointForStopObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
     const Obstacle & obstacle) const;
+  std::optional<CruiseObstacle> createYieldCruiseObstacle(
+    const Obstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points);
+  std::optional<std::vector<CruiseObstacle>> findYieldCruiseObstacles(
+    const std::vector<Obstacle> & obstacles, const std::vector<TrajectoryPoint> & traj_points);
   std::optional<CruiseObstacle> createCruiseObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
     const Obstacle & obstacle, const double precise_lat_dist);
@@ -196,8 +200,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     int successive_num_to_entry_slow_down_condition;
     int successive_num_to_exit_slow_down_condition;
     // consideration for the current ego pose
-    bool enable_to_consider_current_pose{false};
     double time_to_convergence{1.5};
+    bool enable_to_consider_current_pose{false};
+    // yield related parameters
+    bool enable_yield{false};
+    double yield_lat_distance_threshold;
+    double max_lat_dist_between_obstacles;
+    double stopped_obstacle_velocity_threshold;
+    double max_obstacles_collision_time;
   };
   BehaviorDeterminationParam behavior_determination_param_;
 

planning/obstacle_cruise_planner/src/node.cpp

@@ -54,20 +54,15 @@ std::optional<T> getObstacleFromUuid(
   return *itr;
 }
 
-bool isFrontObstacle(
+std::optional<double> calcDistanceToFrontVehicle(
   const std::vector<TrajectoryPoint> & traj_points, const size_t ego_idx,
   const geometry_msgs::msg::Point & obstacle_pos)
 {
   const size_t obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle_pos);
-
-  const double ego_to_obstacle_distance =
+  const auto ego_to_obstacle_distance =
     motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx);
-
-  if (ego_to_obstacle_distance < 0) {
-    return false;
-  }
-
-  return true;
+  if (ego_to_obstacle_distance < 0.0) return std::nullopt;
+  return ego_to_obstacle_distance;
 }
 
 PredictedPath resampleHighestConfidencePredictedPath(
@@ -254,6 +249,15 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     node.declare_parameter<double>("behavior_determination.stop.max_lat_margin");
   max_lat_margin_for_cruise =
     node.declare_parameter<double>("behavior_determination.cruise.max_lat_margin");
+  enable_yield = node.declare_parameter<bool>("behavior_determination.cruise.yield.enable_yield");
+  yield_lat_distance_threshold =
+    node.declare_parameter<double>("behavior_determination.cruise.yield.lat_distance_threshold");
+  max_lat_dist_between_obstacles = node.declare_parameter<double>(
+    "behavior_determination.cruise.yield.max_lat_dist_between_obstacles");
+  stopped_obstacle_velocity_threshold = node.declare_parameter<double>(
+    "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold");
+  max_obstacles_collision_time = node.declare_parameter<double>(
+    "behavior_determination.cruise.yield.max_obstacles_collision_time");
   max_lat_margin_for_slow_down =
     node.declare_parameter<double>("behavior_determination.slow_down.max_lat_margin");
   lat_hysteresis_margin_for_slow_down =
@@ -309,6 +313,20 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
     parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop);
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise);
+  tier4_autoware_utils::updateParam<bool>(
+    parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.cruise.yield.lat_distance_threshold",
+    yield_lat_distance_threshold);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles",
+    max_lat_dist_between_obstacles);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold",
+    stopped_obstacle_velocity_threshold);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time",
+    max_obstacles_collision_time);
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down);
   tier4_autoware_utils::updateParam<double>(
@@ -493,7 +511,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
   //    (3) not too far from trajectory
   const auto target_obstacles = convertToObstacles(traj_points);
 
-  // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
+  //  2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
   const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] =
     determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles);
 
@@ -590,6 +608,7 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
   stop_watch_.tic(__func__);
 
   const auto obj_stamp = rclcpp::Time(objects_ptr_->header.stamp);
+  const auto & p = behavior_determination_param_;
 
   std::vector<Obstacle> target_obstacles;
   for (const auto & predicted_object : objects_ptr_->objects) {
@@ -611,24 +630,25 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
 
     // 2. Check if the obstacle is in front of the ego.
     const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, ego_odom_ptr_->pose.pose);
-    const bool is_front_obstacle =
-      isFrontObstacle(traj_points, ego_idx, current_obstacle_pose.pose.position);
-    if (!is_front_obstacle) {
+    const auto ego_to_obstacle_distance =
+      calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position);
+    if (!ego_to_obstacle_distance) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.",
         object_id.c_str());
       continue;
     }
 
     // 3. Check if rough lateral distance is smaller than the threshold
+    const double lat_dist_from_obstacle_to_traj =
+      motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position);
+
     const double min_lat_dist_to_traj_poly = [&]() {
-      const double lat_dist_from_obstacle_to_traj =
-        motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position);
       const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape);
       return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m -
              obstacle_max_length;
     }();
-    const auto & p = behavior_determination_param_;
+
     const double max_lat_margin = std::max(
       std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise),
       p.max_lat_margin_for_slow_down);
@@ -639,7 +659,9 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
       continue;
     }
 
-    const auto target_obstacle = Obstacle(obj_stamp, predicted_object, current_obstacle_pose.pose);
+    const auto target_obstacle = Obstacle(
+      obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(),
+      lat_dist_from_obstacle_to_traj);
     target_obstacles.push_back(target_obstacle);
   }
 
@@ -741,6 +763,23 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
       continue;
     }
   }
+  const auto & p = behavior_determination_param_;
+  if (p.enable_yield) {
+    const auto yield_obstacles = findYieldCruiseObstacles(obstacles, decimated_traj_points);
+    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);
+        }
+      }
+    }
+  }
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
   // Check target obstacles' consistency
@@ -829,6 +868,137 @@ std::optional<CruiseObstacle> ObstacleCruisePlannerNode::createCruiseObstacle(
                         tangent_vel,   normal_vel,     *collision_points};
 }
 
+std::optional<CruiseObstacle> ObstacleCruisePlannerNode::createYieldCruiseObstacle(
+  const Obstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points)
+{
+  if (traj_points.empty()) return std::nullopt;
+  // check label
+  const auto & object_id = obstacle.uuid.substr(0, 4);
+  const auto & p = behavior_determination_param_;
+
+  if (!isOutsideCruiseObstacle(obstacle.classification.label)) {
+    RCLCPP_INFO_EXPRESSION(
+      get_logger(), enable_debug_info_,
+      "[Cruise] Ignore yield obstacle (%s) since its type is not designated.", object_id.c_str());
+    return std::nullopt;
+  }
+
+  if (
+    std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) <
+    p.outside_obstacle_min_velocity_threshold) {
+    RCLCPP_INFO_EXPRESSION(
+      get_logger(), enable_debug_info_,
+      "[Cruise] Ignore yield obstacle (%s) since the obstacle velocity is low.", object_id.c_str());
+    return std::nullopt;
+  }
+
+  if (isObstacleCrossing(traj_points, obstacle)) {
+    RCLCPP_INFO_EXPRESSION(
+      get_logger(), enable_debug_info_,
+      "[Cruise] Ignore yield obstacle (%s) since it's crossing the ego's trajectory..",
+      object_id.c_str());
+    return std::nullopt;
+  }
+
+  const auto collision_points = [&]() -> std::optional<std::vector<PointWithStamp>> {
+    const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose);
+    if (!obstacle_idx) return std::nullopt;
+    const auto collision_traj_point = traj_points.at(obstacle_idx.value());
+    const auto object_time = now() + traj_points.at(obstacle_idx.value()).time_from_start;
+
+    PointWithStamp collision_traj_point_with_stamp;
+    collision_traj_point_with_stamp.stamp = object_time;
+    collision_traj_point_with_stamp.point.x = collision_traj_point.pose.position.x;
+    collision_traj_point_with_stamp.point.y = collision_traj_point.pose.position.y;
+    collision_traj_point_with_stamp.point.z = collision_traj_point.pose.position.z;
+    std::vector<PointWithStamp> collision_points_vector{collision_traj_point_with_stamp};
+    return collision_points_vector;
+  }();
+
+  if (!collision_points) return std::nullopt;
+  const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
+  // check if obstacle is driving on the opposite direction
+  if (tangent_vel < 0.0) return std::nullopt;
+  return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,
+                        tangent_vel,   normal_vel,     collision_points.value()};
+}
+
+std::optional<std::vector<CruiseObstacle>> ObstacleCruisePlannerNode::findYieldCruiseObstacles(
+  const std::vector<Obstacle> & obstacles, const std::vector<TrajectoryPoint> & traj_points)
+{
+  if (obstacles.empty() || traj_points.empty()) return std::nullopt;
+  const auto & p = behavior_determination_param_;
+
+  std::vector<Obstacle> stopped_obstacles;
+  std::vector<Obstacle> moving_obstacles;
+
+  std::for_each(
+    obstacles.begin(), obstacles.end(),
+    [&stopped_obstacles, &moving_obstacles, &p](const auto & o) {
+      const bool is_moving =
+        std::hypot(o.twist.linear.x, o.twist.linear.y) > p.stopped_obstacle_velocity_threshold;
+      if (is_moving) {
+        const bool is_within_lat_dist_threshold =
+          o.lat_dist_from_obstacle_to_traj < p.yield_lat_distance_threshold;
+        if (is_within_lat_dist_threshold) moving_obstacles.push_back(o);
+        return;
+      }
+      // lat threshold is larger for stopped obstacles
+      const bool is_within_lat_dist_threshold =
+        o.lat_dist_from_obstacle_to_traj <
+        p.yield_lat_distance_threshold + p.max_lat_dist_between_obstacles;
+      if (is_within_lat_dist_threshold) stopped_obstacles.push_back(o);
+      return;
+    });
+
+  if (stopped_obstacles.empty() || moving_obstacles.empty()) return std::nullopt;
+
+  std::sort(
+    stopped_obstacles.begin(), stopped_obstacles.end(), [](const auto & o1, const auto & o2) {
+      return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance;
+    });
+
+  std::sort(moving_obstacles.begin(), moving_obstacles.end(), [](const auto & o1, const auto & o2) {
+    return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance;
+  });
+
+  std::vector<CruiseObstacle> yield_obstacles;
+  for (const auto & moving_obstacle : moving_obstacles) {
+    for (const auto & stopped_obstacle : stopped_obstacles) {
+      const bool is_moving_obs_behind_of_stopped_obs =
+        moving_obstacle.ego_to_obstacle_distance < stopped_obstacle.ego_to_obstacle_distance;
+      const bool is_moving_obs_ahead_of_ego_front =
+        moving_obstacle.ego_to_obstacle_distance > vehicle_info_.vehicle_length_m;
+
+      if (!is_moving_obs_ahead_of_ego_front || !is_moving_obs_behind_of_stopped_obs) continue;
+
+      const double lateral_distance_between_obstacles = std::abs(
+        moving_obstacle.lat_dist_from_obstacle_to_traj -
+        stopped_obstacle.lat_dist_from_obstacle_to_traj);
+
+      const double longitudinal_distance_between_obstacles = std::abs(
+        moving_obstacle.ego_to_obstacle_distance - stopped_obstacle.ego_to_obstacle_distance);
+
+      const double moving_obstacle_speed =
+        std::hypot(moving_obstacle.twist.linear.x, moving_obstacle.twist.linear.y);
+
+      const bool are_obstacles_aligned =
+        lateral_distance_between_obstacles < p.max_lat_dist_between_obstacles;
+      const bool obstacles_collide_within_threshold_time =
+        longitudinal_distance_between_obstacles / moving_obstacle_speed <
+        p.max_obstacles_collision_time;
+      if (are_obstacles_aligned && obstacles_collide_within_threshold_time) {
+        const auto yield_obstacle = createYieldCruiseObstacle(moving_obstacle, traj_points);
+        if (yield_obstacle) {
+          yield_obstacles.push_back(*yield_obstacle);
+        }
+      }
+    }
+  }
+  if (yield_obstacles.empty()) return std::nullopt;
+  return yield_obstacles;
+}
+
 std::optional<std::vector<PointWithStamp>>
 ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,