fix(obstacle_cruise_planner): ignore behind obstacles (autowarefounda…

…tion#1894)

* fix(obstacle_cruise_planner): ignore behind obstacles

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix invalid access

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* update isFrontCollideObstacle

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -101,6 +101,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   bool isCruiseObstacle(const uint8_t label);
   bool isStopObstacle(const uint8_t label);
+  bool isFrontCollideObstacle(
+    const Trajectory & traj, const PredictedObject & object, const size_t first_collision_idx);
 
   bool is_showing_debug_info_;
   double min_behavior_stop_margin_;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp

@@ -48,7 +48,7 @@ std::vector<geometry_msgs::msg::PointStamped> getCollisionPoints(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward,
-  const double max_dist = std::numeric_limits<double>::max(),
+  std::vector<size_t> & collision_index, const double max_dist = std::numeric_limits<double>::max(),
   const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());
 
 std::vector<geometry_msgs::msg::PointStamped> willCollideWithSurroundObstacle(
@@ -57,7 +57,7 @@ std::vector<geometry_msgs::msg::PointStamped> willCollideWithSurroundObstacle(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time,
   const double max_dist, const double ego_obstacle_overlap_time_threshold,
-  const double max_prediction_time_for_collision_check,
+  const double max_prediction_time_for_collision_check, std::vector<size_t> & collision_index,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward);
 
 std::vector<Polygon2d> createOneStepPolygons(

planning/obstacle_cruise_planner/src/node.cpp

@@ -62,12 +62,13 @@ Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx
 }
 
 bool isFrontObstacle(
-  const Trajectory & traj, const size_t ego_idx, const geometry_msgs::msg::Point & obj_pos)
+  const std::vector<TrajectoryPoint> & traj_points, const size_t ego_idx,
+  const geometry_msgs::msg::Point & obj_pos)
 {
-  size_t obj_idx = motion_utils::findNearestSegmentIndex(traj.points, obj_pos);
+  size_t obj_idx = motion_utils::findNearestSegmentIndex(traj_points, obj_pos);
 
   const double ego_to_obj_distance =
-    motion_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx);
+    motion_utils::calcSignedArcLength(traj_points, ego_idx, obj_idx);
 
   if (ego_to_obj_distance < 0) {
     return false;
@@ -608,6 +609,20 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData(
   return planner_data;
 }
 
+bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
+  const Trajectory & traj, const PredictedObject & object, const size_t first_collision_idx)
+{
+  const auto object_pose = object.kinematics.initial_pose_with_covariance.pose;
+  const auto obj_idx = motion_utils::findNearestIndex(traj.points, object_pose.position);
+
+  const double obj_to_col_points_distance =
+    motion_utils::calcSignedArcLength(traj.points, obj_idx, first_collision_idx);
+  const double obj_max_length = calcObjectMaxLength(object.shape);
+
+  // If the object is far in front of the collision point, the object is behind the ego.
+  return obj_to_col_points_distance > -obj_max_length;
+}
+
 ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData(
   const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
   const std::vector<TargetObstacle> & obstacles, const bool is_driving_forward)
@@ -710,8 +725,9 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
     const auto & object_velocity =
       predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x;
 
-    const bool is_front_obstacle =
-      isFrontObstacle(traj, ego_idx, current_object_pose.pose.position);
+    const bool is_front_obstacle = isFrontObstacle(
+      motion_utils::convertToTrajectoryPointArray(traj), ego_idx,
+      current_object_pose.pose.position);
     if (!is_front_obstacle) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), is_showing_debug_info_,
@@ -749,12 +765,13 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
 
     // precise detection area filtering with polygons
     std::vector<geometry_msgs::msg::PointStamped> collision_points;
+    std::vector<size_t> collision_index;
     if (first_within_idx) {  // obstacles inside the trajectory
       // calculate nearest collision point
       collision_points = polygon_utils::getCollisionPoints(
         extended_traj, extended_traj_polygons, predicted_objects.header, resampled_predicted_path,
         predicted_object.shape, current_time, vehicle_info_.max_longitudinal_offset_m,
-        is_driving_forward);
+        is_driving_forward, collision_index);
 
       const bool is_angle_aligned = isAngleAlignedWithTrajectory(
         extended_traj, current_object_pose.pose,
@@ -809,12 +826,13 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
         continue;
       }
 
+      std::vector<size_t> collision_index;
       collision_points = polygon_utils::willCollideWithSurroundObstacle(
         extended_traj, extended_traj_polygons, predicted_objects.header, resampled_predicted_path,
         predicted_object.shape, current_time,
         vehicle_info_.vehicle_width_m + obstacle_filtering_param_.rough_detection_area_expand_width,
         obstacle_filtering_param_.ego_obstacle_overlap_time_threshold,
-        obstacle_filtering_param_.max_prediction_time_for_collision_check,
+        obstacle_filtering_param_.max_prediction_time_for_collision_check, collision_index,
         vehicle_info_.max_longitudinal_offset_m, is_driving_forward);
 
       if (collision_points.empty()) {
@@ -827,6 +845,14 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
         debug_data.intentionally_ignored_obstacles.push_back(predicted_object);
         continue;
       }
+
+      // Ignore obstacles behind the ego vehicle.
+      // Note: Only using isFrontObstacle(), behind obstacles cannot be filtered
+      // properly when the trajectory is crossing or overlapping.
+      const size_t first_collision_index = collision_index.front();
+      if (!isFrontCollideObstacle(extended_traj, predicted_object, first_collision_index)) {
+        continue;
+      }
     }
 
     // For debug

planning/obstacle_cruise_planner/src/polygon_utils.cpp

@@ -133,7 +133,8 @@ std::vector<geometry_msgs::msg::PointStamped> getCollisionPoints(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward,
-  const double max_dist, const double max_prediction_time_for_collision_check)
+  std::vector<size_t> & collision_index, const double max_dist,
+  const double max_prediction_time_for_collision_check)
 {
   std::vector<geometry_msgs::msg::PointStamped> collision_points;
   for (size_t i = 0; i < predicted_path.path.size(); ++i) {
@@ -163,6 +164,7 @@ std::vector<geometry_msgs::msg::PointStamped> getCollisionPoints(
         *collision_idx, current_collision_points, traj, vehicle_max_longitudinal_offset,
         is_driving_forward);
       collision_points.push_back(nearest_collision_point);
+      collision_index.push_back(*collision_idx);
     }
   }
 
@@ -175,12 +177,12 @@ std::vector<geometry_msgs::msg::PointStamped> willCollideWithSurroundObstacle(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time,
   const double max_dist, const double ego_obstacle_overlap_time_threshold,
-  const double max_prediction_time_for_collision_check,
+  const double max_prediction_time_for_collision_check, std::vector<size_t> & collision_index,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward)
 {
   const auto collision_points = getCollisionPoints(
     traj, traj_polygons, obj_header, predicted_path, shape, current_time,
-    vehicle_max_longitudinal_offset, is_driving_forward, max_dist,
+    vehicle_max_longitudinal_offset, is_driving_forward, collision_index, max_dist,
     max_prediction_time_for_collision_check);
 
   if (collision_points.empty()) {