feat(out_of_lane): redesign (#1509)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Tiankui Xian <1041084556@qq.com>
Co-authored-by: Shohei Sakai <saka1s.jp@gmail.com>

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg:
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold = 0.0);
 
+/// @brief calculate the time_from_start fields of the given trajectory points
+/// @details this function assumes constant longitudinal velocity between points
+/// @param trajectory trajectory for which to calculate the time_from_start
+/// @param current_ego_point current ego position
+/// @param min_velocity minimum velocity used for a trajectory point
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f);
+
 }  // namespace autoware::motion_utils
 
 #endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

common/autoware_motion_utils/src/trajectory/trajectory.cpp

@@ -599,4 +599,27 @@ template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg::Trajec
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold);
 
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity)
+{
+  const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point);
+  if (nearest_segment_idx + 1 == trajectory.size()) {
+    return;
+  }
+  for (auto & p : trajectory) {
+    p.time_from_start = rclcpp::Duration::from_seconds(0);
+  }
+  // TODO(Maxime): some points can have very low velocities which introduce huge time errors
+  // Temporary solution: use a minimum velocity
+  for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) {
+    const auto & from = trajectory[idx - 1];
+    const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps);
+    if (velocity != 0.0) {
+      auto & to = trajectory[idx];
+      const auto t = universe_utils::calcDistance2d(from, to) / velocity;
+      to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start;
+    }
+  }
+}
 }  // namespace autoware::motion_utils

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -1,32 +1,22 @@
 /**:
   ros__parameters:
     out_of_lane:  # module to stop or slowdown before overlapping another lane with other objects
-      mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc"
+      mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc"
       skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane
+      max_arc_length: 100.0  # [m] maximum trajectory arc length that is checked for out_of_lane collisions
 
       threshold:
         time_threshold: 5.0  # [s] consider objects that will reach an overlap within this time
-      intervals:  # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego
-        ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer
-        objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer
       ttc:
         threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap
 
       objects:
         minimum_velocity: 0.5  # [m/s] objects lower than this velocity will be ignored
-        use_predicted_paths: true  # if true, use the predicted paths to estimate future positions.
-                                   # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
         predicted_path_min_confidence : 0.1  # when using predicted paths, ignore the ones whose confidence is lower than this value.
-        distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
         cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
         ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored
 
-      overlap:
-        minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
-        extra_length: 0.0  # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times)
-
       action:  # action to insert in the trajectory if an object causes a conflict at an overlap
-        skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
         precision: 0.1  # [m] precision when inserting a stop pose in the trajectory
         longitudinal_distance_buffer: 1.5  # [m] safety distance buffer to keep in front of the ego vehicle
         lateral_distance_buffer: 1.0  # [m] safety distance buffer to keep on the side of the ego vehicle
@@ -38,8 +28,8 @@
           distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap
 
       ego:
-        min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego
-        extra_front_offset: 0.0 # [m] extra front distance
-        extra_rear_offset: 0.0 # [m] extra rear distance
-        extra_right_offset: 0.0 # [m] extra right distance
-        extra_left_offset: 0.0 # [m] extra left distance
+        # extra footprint offsets to calculate out of lane collisions
+        extra_front_offset: 0.0 # [m] extra footprint front distance
+        extra_rear_offset: 0.0 # [m] extra footprint rear distance
+        extra_right_offset: 0.0 # [m] extra footprint right distance
+        extra_left_offset: 0.0 # [m] extra footprint left distance

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp

@@ -15,74 +15,117 @@
 #include "calculate_slowdown_points.hpp"
 
 #include "footprint.hpp"
+#include "types.hpp"
 
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 
-#include <boost/geometry/algorithms/overlaps.hpp>
+#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 
+#include <boost/geometry/algorithms/disjoint.hpp>
+
+#include <lanelet2_core/Forward.h>
 #include <lanelet2_core/geometry/Polygon.h>
 
+#include <algorithm>
+#include <optional>
+#include <vector>
+
 namespace autoware::motion_velocity_planner::out_of_lane
 {
 
-bool can_decelerate(
-  const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel)
+std::optional<geometry_msgs::msg::Pose> calculate_last_avoiding_pose(
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid,
+  const double min_arc_length, const double max_arc_length, const double precision)
 {
-  // TODO(Maxime): use the library function
-  const auto dist_ahead_of_ego = autoware::motion_utils::calcSignedArcLength(
-    ego_data.trajectory_points, ego_data.pose.position, point.pose.position);
-  const auto acc_to_target_vel =
-    (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego);
-  return acc_to_target_vel < std::abs(ego_data.max_decel);
+  geometry_msgs::msg::Pose interpolated_pose{};
+  bool is_avoiding_pose = false;
+
+  auto from = min_arc_length;
+  auto to = max_arc_length;
+  while (to - from > precision) {
+    auto l = from + 0.5 * (to - from);
+    interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l);
+    const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
+    is_avoiding_pose =
+      std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) {
+        return boost::geometry::disjoint(interpolated_footprint, polygon);
+      });
+    if (is_avoiding_pose) {
+      from = l;
+    } else {
+      to = l;
+    }
+  }
+  if (is_avoiding_pose) {
+    return interpolated_pose;
+  }
+  return std::nullopt;
 }
 
-std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
-  const EgoData & ego_data, const Slowdown & decision,
-  const autoware::universe_utils::Polygon2d & footprint,
-  const std::optional<SlowdownToInsert> & prev_slowdown_point, const PlannerParam & params)
+std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
+  const EgoData & ego_data, const OutOfLanePoint & point_to_avoid,
+  const universe_utils::Polygon2d & footprint, const double precision)
 {
-  const auto from_arc_length = autoware::motion_utils::calcSignedArcLength(
-    ego_data.trajectory_points, 0, ego_data.first_trajectory_idx);
-  const auto to_arc_length = autoware::motion_utils::calcSignedArcLength(
-    ego_data.trajectory_points, 0, decision.target_trajectory_idx);
-  TrajectoryPoint interpolated_point;
-  for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) {
-    // TODO(Maxime): binary search
-    interpolated_point.pose =
-      autoware::motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l);
-    const auto respect_decel_limit =
-      !params.skip_if_over_max_decel || prev_slowdown_point ||
-      can_decelerate(ego_data, interpolated_point, decision.velocity);
-    const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose);
-    const auto is_overlap_lane = boost::geometry::overlaps(
-      interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon());
-    const auto is_overlap_extra_lane =
-      prev_slowdown_point &&
-      boost::geometry::overlaps(
-        interpolated_footprint,
-        prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon());
-    if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane)
-      return interpolated_point;
+  const auto first_avoid_arc_length = motion_utils::calcSignedArcLength(
+    ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index);
+  for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length;
+       l -= precision) {
+    const auto interpolated_pose =
+      motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l);
+    const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
+    if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) {
+      return interpolated_pose;
+    }
   }
   return std::nullopt;
 }
 
-std::optional<SlowdownToInsert> calculate_slowdown_point(
-  const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params)
+std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
+  const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params)
 {
+  const auto point_to_avoid_it = std::find_if(
+    out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(),
+    [&](const auto & p) { return p.to_avoid; });
+  if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) {
+    return std::nullopt;
+  }
+  const auto raw_footprint = make_base_footprint(params, true);  // ignore extra footprint offsets
+  const auto base_footprint = make_base_footprint(params);
   params.extra_front_offset += params.lon_dist_buffer;
   params.extra_right_offset += params.lat_dist_buffer;
   params.extra_left_offset += params.lat_dist_buffer;
-  const auto base_footprint = make_base_footprint(params);
+  const auto expanded_footprint = make_base_footprint(params);  // with added distance buffers
+  lanelet::BasicPolygons2d polygons_to_avoid;
+  for (const auto & ll : point_to_avoid_it->overlapped_lanelets) {
+    polygons_to_avoid.push_back(ll.polygon2d().basicPolygon());
+  }
+  // points are ordered by trajectory index so the first one has the smallest index and arc length
+  const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index;
+  const auto first_outside_arc_length =
+    motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx);
 
+  std::optional<geometry_msgs::msg::Pose> slowdown_point;
   // search for the first slowdown decision for which a stop point can be inserted
-  for (const auto & decision : decisions) {
-    const auto last_in_lane_pose =
-      calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params);
-    if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose};
+  // we first try to use the expanded footprint (distance buffers + extra footprint offsets)
+  for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) {
+    slowdown_point = calculate_last_avoiding_pose(
+      ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length,
+      first_outside_arc_length, params.precision);
+    if (slowdown_point) {
+      break;
+    }
   }
-  return std::nullopt;
+  // fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or
+  // not)
+  if (!slowdown_point) {
+    slowdown_point = calculate_pose_ahead_of_collision(
+      ego_data, *point_to_avoid_it, expanded_footprint, params.precision);
+  }
+  return slowdown_point;
 }
 }  // namespace autoware::motion_velocity_planner::out_of_lane

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -19,40 +19,39 @@
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <geometry_msgs/msg/pose.hpp>
+
 #include <optional>
-#include <vector>
 
 namespace autoware::motion_velocity_planner::out_of_lane
 {
-
-/// @brief estimate whether ego can decelerate without breaking the deceleration limit
-/// @details assume ego wants to reach the target point at the target velocity
-/// @param [in] ego_data ego data
-/// @param [in] point target point
-/// @param [in] target_vel target_velocity
-bool can_decelerate(
-  const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel);
-
-/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid
+/// @brief calculate the last pose along the trajectory where ego does not go out of lane
 /// @param [in] ego_data ego data
-/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed)
 /// @param [in] footprint the ego footprint
-/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits
-/// @param [in] params parameters
+/// @param [in] min_arc_length minimum arc length for the search
+/// @param [in] max_arc_length maximum arc length for the search
+/// @param [in] precision [m] search precision
 /// @return the last pose that is not out of lane (if found)
-std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
-  const EgoData & ego_data, const Slowdown & decision,
-  const autoware::universe_utils::Polygon2d & footprint,
-  const std::optional<SlowdownToInsert> & prev_slowdown_point, const PlannerParam & params);
+std::optional<geometry_msgs::msg::Pose> calculate_last_in_lane_pose(
+  const EgoData & ego_data, const autoware::universe_utils::Polygon2d & footprint,
+  const double min_arc_length, const double max_arc_length, const double precision);
+
+/// @brief calculate the slowdown pose just ahead of a point to avoid
+/// @param ego_data ego data (trajectory, velocity, etc)
+/// @param point_to_avoid the point to avoid
+/// @param footprint the ego footprint
+/// @param params parameters
+/// @return optional slowdown point to insert in the trajectory
+std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
+  const EgoData & ego_data, const OutOfLanePoint & point_to_avoid,
+  const universe_utils::Polygon2d & footprint, const double precision);
 
 /// @brief calculate the slowdown point to insert in the trajectory
 /// @param ego_data ego data (trajectory, velocity, etc)
-/// @param decisions decision (before which point to stop, what lane to avoid entering, etc)
-/// @param prev_slowdown_point previously calculated slowdown point
+/// @param out_of_lane_data data about out of lane areas
 /// @param params parameters
 /// @return optional slowdown point to insert in the trajectory
-std::optional<SlowdownToInsert> calculate_slowdown_point(
-  const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params);
+std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
+  const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params);
 }  // namespace autoware::motion_velocity_planner::out_of_lane
 #endif  // CALCULATE_SLOWDOWN_POINTS_HPP_