fix(obstacle_cruise_planner): fix some bugs

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp

@@ -46,13 +46,14 @@ boost::optional<size_t> getFirstNonCollisionIndex(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, const size_t start_idx);
 
-bool willCollideWithSurroundObstacle(
+boost::optional<size_t> willCollideWithSurroundObstacle(
   const autoware_auto_planning_msgs::msg::Trajectory & traj,
   const std::vector<Polygon2d> & traj_polygons,
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, 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<geometry_msgs::msg::Point> & collision_geom_points);
 
 std::vector<Polygon2d> createOneStepPolygons(
   const autoware_auto_planning_msgs::msg::Trajectory & traj,

planning/obstacle_cruise_planner/src/node.cpp

@@ -67,7 +67,7 @@ bool isFrontObstacle(
   const double ego_to_obj_distance =
     tier4_autoware_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx);
 
-  if (obj_idx == 0 && ego_to_obj_distance < 0) {
+  if (ego_to_obj_distance < 0) {
     return false;
   }
 
@@ -537,7 +537,7 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
     }();
     if (
       std::fabs(dist_from_obstacle_to_traj) >
-      obstacle_filtering_param_.rough_detection_area_expand_width) {
+      vehicle_info_.vehicle_width_m + obstacle_filtering_param_.rough_detection_area_expand_width) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), is_showing_debug_info_,
         "Ignore obstacles since it is far from the trajectory.");
@@ -593,16 +593,15 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
       const auto predicted_path_with_highest_confidence =
         getHighestConfidencePredictedPath(predicted_object);
 
-      const bool will_collide = polygon_utils::willCollideWithSurroundObstacle(
+      std::vector<geometry_msgs::msg::Point> future_collision_points;
+      const auto collision_traj_poly_idx = polygon_utils::willCollideWithSurroundObstacle(
         decimated_traj, decimated_traj_polygons, predicted_path_with_highest_confidence,
-        predicted_object.shape, obstacle_filtering_param_.rough_detection_area_expand_width,
+        predicted_object.shape,
+        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, future_collision_points);
 
-      // TODO(murooka) think later
-      nearest_collision_point = object_pose.position;
-
-      if (!will_collide) {
+      if (!collision_traj_poly_idx) {
         // Ignore condition 2
         // Ignore vehicle obstacles outside the trajectory, whose predicted path
         // overlaps the ego trajectory in a certain time.
@@ -613,6 +612,10 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
         debug_data.intentionally_ignored_obstacles.push_back(predicted_object);
         continue;
       }
+
+      nearest_collision_point = calcNearestCollisionPoint(
+        collision_traj_poly_idx.get(), future_collision_points, decimated_traj);
+      debug_data.collision_points.push_back(nearest_collision_point);
     }
 
     // convert to obstacle type

planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -231,7 +231,7 @@ void PIDBasedPlanner::calcObstaclesToCruiseAndStop(
       const double error_dist = dist_to_obstacle - longitudinal_info_.safe_distance_margin;
       if (stop_obstacle_info) {
         if (error_dist > stop_obstacle_info->dist_to_stop) {
-          return;
+          continue;
         }
       }
       stop_obstacle_info = StopObstacleInfo(obstacle, error_dist);
@@ -253,7 +253,7 @@ void PIDBasedPlanner::calcObstaclesToCruiseAndStop(
       const double error_dist = dist_to_obstacle - rss_dist;
       if (cruise_obstacle_info) {
         if (error_dist > cruise_obstacle_info->dist_to_cruise) {
-          return;
+          continue;
         }
       }
       const double normalized_dist_to_cruise = error_dist / dist_to_obstacle;
@@ -400,8 +400,7 @@ boost::optional<size_t> PIDBasedPlanner::doStop(
   const auto marker_pose = obstacle_cruise_utils::calcForwardPose(
     planner_data.traj, ego_idx, modified_dist_to_stop + vehicle_info_.max_longitudinal_offset_m);
   if (marker_pose) {
-    visualization_msgs::msg::MarkerArray wall_msg;
-    const auto markers = tier4_autoware_utils::createStopVirtualWallMarker(
+    auto markers = tier4_autoware_utils::createStopVirtualWallMarker(
       marker_pose.get(), "obstacle stop", planner_data.current_time, 0);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker);
   }

planning/obstacle_cruise_planner/src/polygon_utils.cpp

@@ -235,13 +235,14 @@ boost::optional<size_t> getFirstNonCollisionIndex(
   return {};
 }
 
-bool willCollideWithSurroundObstacle(
+boost::optional<size_t> willCollideWithSurroundObstacle(
   const autoware_auto_planning_msgs::msg::Trajectory & traj,
   const std::vector<Polygon2d> & traj_polygons,
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const autoware_auto_perception_msgs::msg::Shape & shape, 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<geometry_msgs::msg::Point> & collision_geom_points)
 {
   constexpr double epsilon = 1e-3;
 
@@ -252,7 +253,7 @@ bool willCollideWithSurroundObstacle(
     if (
       max_prediction_time_for_collision_check <
       rclcpp::Duration(predicted_path.time_step).seconds() * static_cast<double>(i)) {
-      return false;
+      return {};
     }
 
     for (size_t j = 0; j < traj.points.size(); ++j) {
@@ -269,13 +270,33 @@ bool willCollideWithSurroundObstacle(
 
       if (dist < epsilon) {
         if (!is_found) {
-          start_predicted_path_idx = i;
-          is_found = true;
+          // calculate collision point by polygon collision
+          std::deque<Polygon2d> collision_polygons;
+          boost::geometry::intersection(traj_polygon, obj_polygon, collision_polygons);
+
+          bool has_collision = false;
+          for (const auto & collision_polygon : collision_polygons) {
+            if (boost::geometry::area(collision_polygon) > 0.0) {
+              has_collision = true;
+
+              for (const auto & collision_point : collision_polygon.outer()) {
+                geometry_msgs::msg::Point collision_geom_point;
+                collision_geom_point.x = collision_point.x();
+                collision_geom_point.y = collision_point.y();
+                collision_geom_points.push_back(collision_geom_point);
+              }
+            }
+          }
+
+          if (has_collision) {
+            start_predicted_path_idx = i;
+            is_found = true;
+          }
         } else {
           const double overlap_time = (static_cast<double>(i) - start_predicted_path_idx) *
                                       rclcpp::Duration(predicted_path.time_step).seconds();
           if (ego_obstacle_overlap_time_threshold < overlap_time) {
-            return true;
+            return j;
           }
         }
       } else {
@@ -284,8 +305,10 @@ bool willCollideWithSurroundObstacle(
     }
   }
 
-  return false;
+  collision_geom_points.clear();
+  return {};
 }
+
 std::vector<Polygon2d> createOneStepPolygons(
   const autoware_auto_planning_msgs::msg::Trajectory & traj,
   const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width)

planning/obstacle_cruise_planner/src/utils.cpp

@@ -35,7 +35,6 @@ visualization_msgs::msg::Marker getObjectMarker(
     tier4_autoware_utils::createMarkerScale(2.0, 2.0, 2.0),
     tier4_autoware_utils::createMarkerColor(r, g, b, 0.8));
 
-  marker.lifetime = rclcpp::Duration::from_seconds(0.8);
   marker.pose = obstacle_pose;
 
   return marker;