feat(obstacle_cruise_planner): update obstacle cruise planner (tier4#…

…1531)

* delete is on ego traj

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -50,8 +50,8 @@ struct TargetObstacle
 {
   TargetObstacle(
     const rclcpp::Time & arg_time_stamp, const PredictedObject & object,
-    const double aligned_velocity, const geometry_msgs::msg::PointStamped & arg_collision_point,
-    const bool arg_is_on_ego_trajectory)
+    const double aligned_velocity,
+    const std::vector<geometry_msgs::msg::PointStamped> & arg_collision_points)
   {
     time_stamp = arg_time_stamp;
     orientation_reliable = true;
@@ -68,9 +68,8 @@ struct TargetObstacle
       predicted_paths.push_back(path);
     }
 
-    collision_point = arg_collision_point;
+    collision_points = arg_collision_points;
     has_stopped = false;
-    is_on_ego_trajectory = arg_is_on_ego_trajectory;
   }
 
   rclcpp::Time time_stamp;
@@ -83,9 +82,8 @@ struct TargetObstacle
   Shape shape;
   std::string uuid;
   std::vector<PredictedPath> predicted_paths;
-  geometry_msgs::msg::PointStamped collision_point;
+  std::vector<geometry_msgs::msg::PointStamped> collision_points;
   bool has_stopped;
-  bool is_on_ego_trajectory;
 };
 
 struct ObstacleCruisePlannerData

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -90,7 +90,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   void checkConsistency(
     const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
     const Trajectory & traj, std::vector<TargetObstacle> & target_obstacles);
-  geometry_msgs::msg::PointStamped calcNearestCollisionPoint(
+  std::vector<geometry_msgs::msg::PointStamped> calcNearestCollisionPoint(
     const size_t & first_within_idx,
     const std::vector<geometry_msgs::msg::PointStamped> & collision_points,
     const Trajectory & decimated_traj, const bool is_driving_forward);

planning/obstacle_cruise_planner/src/node.cpp

@@ -686,17 +686,20 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
     // calculate collision points
     const auto obstacle_polygon =
       tier4_autoware_utils::toPolygon2d(object_pose.pose, predicted_object.shape);
-    std::vector<geometry_msgs::msg::PointStamped> collision_points;
+    std::vector<geometry_msgs::msg::PointStamped> closest_collision_points;
     const auto first_within_idx = polygon_utils::getFirstCollisionIndex(
-      decimated_traj_polygons, obstacle_polygon, predicted_objects.header, collision_points);
+      decimated_traj_polygons, obstacle_polygon, predicted_objects.header,
+      closest_collision_points);
 
     // precise detection area filtering with polygons
-    geometry_msgs::msg::PointStamped nearest_collision_point;
+    std::vector<geometry_msgs::msg::PointStamped> collision_points;
     if (first_within_idx) {  // obstacles inside the trajectory
       // calculate nearest collision point
-      nearest_collision_point = calcNearestCollisionPoint(
-        first_within_idx.get(), collision_points, decimated_traj, is_driving_forward);
-      debug_data.collision_points.push_back(nearest_collision_point.point);
+      collision_points = calcNearestCollisionPoint(
+        first_within_idx.get(), closest_collision_points, decimated_traj, is_driving_forward);
+      if (!collision_points.empty()) {
+        debug_data.collision_points.push_back(collision_points.front().point);
+      }
 
       const bool is_angle_aligned = isAngleAlignedWithTrajectory(
         decimated_traj, object_pose.pose,
@@ -705,9 +708,9 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
         std::abs(object_velocity) > obstacle_filtering_param_.crossing_obstacle_velocity_threshold;
 
       // ignore running vehicle crossing the ego trajectory with high speed with some condition
-      if (!is_angle_aligned && has_high_speed) {
+      if (!is_angle_aligned && has_high_speed && !collision_points.empty()) {
         const double collision_time_margin = calcCollisionTimeMargin(
-          current_pose, current_vel, nearest_collision_point.point, predicted_object,
+          current_pose, current_vel, collision_points.front().point, predicted_object,
           first_within_idx.get(), decimated_traj, decimated_traj_polygons, is_driving_forward);
         if (collision_time_margin > obstacle_filtering_param_.collision_time_margin) {
           // Ignore vehicle obstacles inside the trajectory, which is crossing the trajectory with
@@ -765,18 +768,18 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
         continue;
       }
 
-      nearest_collision_point = calcNearestCollisionPoint(
+      collision_points = calcNearestCollisionPoint(
         collision_traj_poly_idx.get(), future_collision_points, decimated_traj, is_driving_forward);
-      debug_data.collision_points.push_back(nearest_collision_point.point);
+      if (!collision_points.empty()) {
+        debug_data.collision_points.push_back(collision_points.front().point);
+      }
     }
 
     // convert to obstacle type
-    const bool is_on_ego_traj = first_within_idx ? true : false;
     const double trajectory_aligned_adaptive_cruise =
       calcAlignedAdaptiveCruise(predicted_object, traj);
     const auto target_obstacle = TargetObstacle(
-      time_stamp, predicted_object, trajectory_aligned_adaptive_cruise, nearest_collision_point,
-      is_on_ego_traj);
+      time_stamp, predicted_object, trajectory_aligned_adaptive_cruise, collision_points);
     target_obstacles.push_back(target_obstacle);
   }
 
@@ -895,7 +898,7 @@ void ObstacleCruisePlannerNode::checkConsistency(
   }
 }
 
-geometry_msgs::msg::PointStamped ObstacleCruisePlannerNode::calcNearestCollisionPoint(
+std::vector<geometry_msgs::msg::PointStamped> ObstacleCruisePlannerNode::calcNearestCollisionPoint(
   const size_t & first_within_idx,
   const std::vector<geometry_msgs::msg::PointStamped> & collision_points,
   const Trajectory & decimated_traj, const bool is_driving_forward)
@@ -931,7 +934,7 @@ geometry_msgs::msg::PointStamped ObstacleCruisePlannerNode::calcNearestCollision
     }
   }
 
-  return collision_points.at(min_idx);
+  return {collision_points.at(min_idx)};
 }
 
 double ObstacleCruisePlannerNode::calcCollisionTimeMargin(

planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -124,6 +124,10 @@ void PIDBasedPlanner::calcObstaclesToCruise(
   for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) {
     const auto & obstacle = planner_data.target_obstacles.at(o_idx);
 
+    if (obstacle.collision_points.empty()) {
+      continue;
+    }
+
     // NOTE: from ego's front to obstacle's back
     const double dist_to_obstacle = calcDistanceToObstacle(planner_data, obstacle);
 
@@ -167,7 +171,7 @@ double PIDBasedPlanner::calcDistanceToObstacle(
   const double offset = abs_ego_offset + segment_offset;
 
   return motion_utils::calcSignedArcLength(
-           planner_data.traj.points, ego_segment_idx, obstacle.collision_point.point) -
+           planner_data.traj.points, ego_segment_idx, obstacle.collision_points.front().point) -
          offset;
 }
 

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -37,7 +37,7 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(
   // create stop factor
   tier4_planning_msgs::msg::StopFactor stop_factor;
   stop_factor.stop_pose = stop_pose;
-  stop_factor.stop_factor_points.emplace_back(stop_obstacle.collision_point.point);
+  stop_factor.stop_factor_points.emplace_back(stop_obstacle.collision_points.front().point);
 
   // create stop reason stamped
   tier4_planning_msgs::msg::StopReason stop_reason_msg;
@@ -102,7 +102,7 @@ Trajectory PlannerInterface::generateStopTrajectory(
   // Get Closest Stop Obstacle
   const auto closest_stop_obstacle =
     obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj, planner_data.target_obstacles);
-  if (!closest_stop_obstacle) {
+  if (!closest_stop_obstacle && closest_stop_obstacle->collision_points.empty()) {
     // delete marker
     const auto markers =
       motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
@@ -113,7 +113,7 @@ Trajectory PlannerInterface::generateStopTrajectory(
 
   // Get Closest Obstacle Stop Distance
   const double closest_obstacle_dist = motion_utils::calcSignedArcLength(
-    planner_data.traj.points, 0, closest_stop_obstacle->collision_point.point);
+    planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point);
 
   const auto negative_dist_to_ego = motion_utils::calcSignedArcLength(
     planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_,

planning/obstacle_cruise_planner/src/utils.cpp

@@ -154,12 +154,12 @@ boost::optional<TargetObstacle> getClosestStopObstacle(
   double dist_to_closest_stop_obstacle = std::numeric_limits<double>::max();
   for (const auto & obstacle : target_obstacles) {
     // Ignore obstacle that has not stopped
-    if (!obstacle.has_stopped) {
+    if (!obstacle.has_stopped || obstacle.collision_points.empty()) {
       continue;
     }
 
     const double dist_to_stop_obstacle =
-      motion_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_point.point);
+      motion_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_points.front().point);
     if (dist_to_stop_obstacle < dist_to_closest_stop_obstacle) {
       dist_to_closest_stop_obstacle = dist_to_stop_obstacle;
       closest_stop_obstacle = obstacle;