works well but may be degraded

Signed-off-by: Yuki Takagi <[email protected]>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -110,14 +110,17 @@ struct StopObstacle : public TargetObstacleInterface
     const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
     const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape,
     const double arg_lon_velocity, const double arg_lat_velocity,
-    const geometry_msgs::msg::Point arg_collision_point)
+    const geometry_msgs::msg::Point arg_collision_point,
+    const double arg_traj_dist_to_stop_point)
   : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
     shape(arg_shape),
-    collision_point(arg_collision_point)
+    collision_point(arg_collision_point),
+    traj_dist_to_stop_point(arg_traj_dist_to_stop_point)
   {
   }
   Shape shape;
   geometry_msgs::msg::Point collision_point;
+  double traj_dist_to_stop_point;
 };
 
 struct CruiseObstacle : public TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -67,6 +67,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   bool isOutsideCruiseObstacle(const uint8_t label) const;
   bool isCruiseObstacle(const uint8_t label) const;
   bool isSlowDownObstacle(const uint8_t label) const;
+  std::optional<double> calcTrajDistToSecurePolysDist(
+    const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
+    const Obstacle & obstacle) const;
   std::optional<geometry_msgs::msg::Point> createCollisionPointForStopObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
     const Obstacle & obstacle) const;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -102,6 +102,11 @@ class PlannerInterface
     slow_down_debug_multi_array_.stamp = current_time;
     return slow_down_debug_multi_array_;
   }
+  double getSafeDistanceMargin()
+  {
+    return longitudinal_info_.safe_distance_margin;
+  }
+
 
 protected:
   // Parameters

planning/obstacle_cruise_planner/src/node.cpp

@@ -24,10 +24,42 @@
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
 #include <boost/format.hpp>
+#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
 
 #include <algorithm>
 #include <chrono>
 
+#define debug(var)                                                      \
+  do {                                                                  \
+    std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \
+    view(var);                                                          \
+  } while (0)
+template <typename T>
+void view(T e)
+{
+  std::cerr << e << std::endl;
+}
+template <typename T>
+void view(const std::vector<T> & v)
+{
+  for (const auto & e : v) {
+    std::cerr << e << " ";
+  }
+  std::cerr << std::endl;
+}
+template <typename T>
+void view(const std::vector<std::vector<T>> & vv)
+{
+  for (const auto & v : vv) {
+    view(v);
+  }
+}
+#define line()                                                                         \
+  {                                                                                    \
+    std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
+  }
+
 namespace
 {
 VelocityLimitClearCommand createVelocityLimitClearCommandMessage(
@@ -997,9 +1029,88 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
     return std::nullopt;
   }
 
+  const auto traj_dist_to_secure_stop_point =
+    calcTrajDistToSecurePolysDist(traj_points, traj_polys, obstacle);
+  if (!traj_dist_to_secure_stop_point) {
+    return std::nullopt;
+  }
+  debug(traj_dist_to_secure_stop_point.value());
+
   const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
-  return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,   obstacle.shape,
-                      tangent_vel,   normal_vel,     *collision_point};
+  return StopObstacle{
+    obstacle.uuid, obstacle.stamp, obstacle.pose,    obstacle.shape,
+    tangent_vel,   normal_vel,     *collision_point, *traj_dist_to_secure_stop_point};
+}
+
+std::optional<double> ObstacleCruisePlannerNode::calcTrajDistToSecurePolysDist(
+  const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
+  const Obstacle & obstacle) const
+{
+  const auto & p = behavior_determination_param_;
+  const auto & object_id = obstacle.uuid.substr(0, 4);
+
+  // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed,
+  //       and the collision between ego and obstacles are within the margin threshold.
+  const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle);
+  const double has_high_speed = p.crossing_obstacle_velocity_threshold <
+                                std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y);
+  if (is_obstacle_crossing && has_high_speed) {
+    // Get highest confidence predicted path
+    const auto resampled_predicted_path = resampleHighestConfidencePredictedPath(
+      obstacle.predicted_paths, p.prediction_resampling_time_interval,
+      p.prediction_resampling_time_horizon);
+
+    std::vector<size_t> collision_index;
+    const auto collision_points = polygon_utils::getCollisionPoints(
+      traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(),
+      is_driving_forward_, collision_index);
+    if (collision_points.empty()) {
+      RCLCPP_INFO_EXPRESSION(
+        get_logger(), enable_debug_info_,
+        "[Stop] Ignore inside obstacle (%s) since there is no collision point between the "
+        "predicted path "
+        "and the ego.",
+        object_id.c_str());
+      debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle);
+      return std::nullopt;
+    }
+
+    const double collision_time_margin =
+      calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_);
+    if (p.collision_time_margin < collision_time_margin) {
+      RCLCPP_INFO_EXPRESSION(
+        get_logger(), enable_debug_info_,
+        "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.",
+        object_id.c_str());
+      debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle);
+      return std::nullopt;
+    }
+  }
+
+  const auto traj_polys_with_lat_margin = createOneStepPolygons(
+    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop);
+  auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);
+  const auto dist_to_be_secured = planner_ptr_->getSafeDistanceMargin();
+
+  for (size_t col_i = 0; col_i < traj_polys_with_lat_margin.size(); ++col_i) {
+    if (boost::geometry::intersects(traj_polys_with_lat_margin.at(col_i), obstacle_polygon)) {
+      double last_dist = 0.0;
+      for (std::int32_t sec_i = col_i - 1; sec_i >= 0; --sec_i) {
+        double current_dist =
+          boost::geometry::distance(traj_polys_with_lat_margin.at(sec_i), obstacle_polygon);
+        debug(current_dist);
+        if (current_dist > dist_to_be_secured) {
+          debug(sec_i);
+          double decimal_secure_index =
+            sec_i + (dist_to_be_secured - current_dist) / (last_dist - current_dist);
+          return decimal_secure_index * p.decimate_trajectory_step_length;
+        }
+        last_dist = current_dist;
+      }
+      return 0.0;
+    }
+  }
+  return std::nullopt;
 }
 
 std::optional<geometry_msgs::msg::Point>