feat(out_of_lane): more stable decisions

Signed-off-by: Maxime CLEMENT <[email protected]>

common/autoware_auto_geometry/CMakeLists.txt

@@ -12,12 +12,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/bounding_box.cpp
 )
 
-if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-  target_compile_definitions(${PROJECT_NAME} PRIVATE
-    DEFINE_LEGACY_FUNCTION
-  )
-endif()
-
 if(BUILD_TESTING)
   set(GEOMETRY_GTEST geometry_gtest)
   set(GEOMETRY_SRC test/src/test_geometry.cpp

common/autoware_auto_tf2/CMakeLists.txt

@@ -17,11 +17,6 @@ if(BUILD_TESTING)
     "tf2_geometry_msgs"
     "tf2_ros"
   )
-  if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-    target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE
-      DEFINE_LEGACY_FUNCTION
-    )
-  endif()
 endif()
 
 ament_auto_package()

common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp

@@ -45,57 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
-#ifdef DEFINE_LEGACY_FUNCTION
-/*************/
-/** Point32 **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The point to transform, as a Point32 message.
- * \param t_out The transformed point, as a Point32 message.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
-  t_out.x = static_cast<float>(v_out[0]);
-  t_out.y = static_cast<float>(v_out[1]);
-  t_out.z = static_cast<float>(v_out[2]);
-}
-
-/*************/
-/** Polygon **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The polygon to transform.
- * \param t_out The transformed polygon.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  // Don't call the Point32 doTransform to avoid doing this conversion every time
-  const auto kdl_frame = gmTransformToKDL(transform);
-  // We don't use std::back_inserter to allow aliasing between t_in and t_out
-  t_out.points.resize(t_in.points.size());
-  for (size_t i = 0; i < t_in.points.size(); ++i) {
-    const KDL::Vector v_out =
-      kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
-    t_out.points[i].x = static_cast<float>(v_out[0]);
-    t_out.points[i].y = static_cast<float>(v_out[1]);
-    t_out.points[i].z = static_cast<float>(v_out[2]);
-  }
-}
-#endif
-
 /******************/
 /** Quaternion32 **/
 /******************/

planning/behavior_path_planner/CMakeLists.txt

@@ -57,12 +57,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/marker_utils/lane_change/debug.cpp
 )
 
-if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-  target_compile_definitions(behavior_path_planner_node PRIVATE
-    DEFINE_LEGACY_FUNCTION
-  )
-endif()
-
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
   ${EIGEN3_INCLUDE_DIR}
 )

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -26,6 +26,15 @@
 
 #include <boost/geometry.hpp>
 
+// for writing the svg file
+#include <fstream>
+#include <iostream>
+// for the geometry types
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+// for the svg mapper
+#include <boost/geometry/io/svg/svg_mapper.hpp>
+#include <boost/geometry/io/svg/write.hpp>
+
 namespace drivable_area_expansion
 {
 
@@ -57,6 +66,22 @@ void expandDrivableArea(
 {
   if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return;
 
+  // Declare a stream and an SVG mapper
+  std::ofstream svg1("/home/mclement/Pictures/DA_before.svg");  // /!\ CHANGE PATH
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> before(svg1, 400, 400);
+  // Declare a stream and an SVG mapper
+  std::ofstream svg2("/home/mclement/Pictures/DA_after.svg");  // /!\ CHANGE PATH
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> after(svg2, 400, 400);
+
+  linestring_t left_ls;
+  linestring_t right_ls;
+  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  before.add(left_ls);
+  before.add(right_ls);
+  before.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  before.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stopwatch;
   const auto & params = planner_data->drivable_area_expansion_parameters;
   const auto & dynamic_objects = *planner_data->dynamic_object;
@@ -126,6 +151,15 @@ void expandDrivableArea(
     std::printf("\tupdate: %2.2fms\n", update_duration);
     std::cout << std::endl;
   }
+
+  left_ls.clear();
+  right_ls.clear();
+  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  after.add(left_ls);
+  after.add(right_ls);
+  after.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  after.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
 }
 
 point_t convert_point(const Point & p)

planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -17,6 +17,7 @@
         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
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
@@ -26,6 +27,7 @@
         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 path
         distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
           velocity: 2.0  # [m/s] slowdown velocity

planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -26,6 +26,11 @@
 namespace behavior_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 PathPointWithLaneId & point, const double target_vel)
 {
@@ -37,9 +42,16 @@ bool can_decelerate(
   return acc_to_target_vel < std::abs(ego_data.max_decel);
 }
 
+/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid
+/// @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
+/// @return the last pose that is not out of lane (if found)
 std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
   const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint,
-  const PlannerParam & params)
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, const PlannerParam & params)
 {
   const auto from_arc_length =
     motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx);
@@ -50,12 +62,17 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
     // TODO(Maxime): binary search
     interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l);
     const auto respect_decel_limit =
-      !params.skip_if_over_max_decel ||
+      !params.skip_if_over_max_decel || prev_slowdown_point ||
       can_decelerate(ego_data, interpolated_point, decision.velocity);
-    if (
-      respect_decel_limit && !boost::geometry::overlaps(
-                               project_to_pose(footprint, interpolated_point.point.pose),
-                               decision.lane_to_avoid.polygon2d().basicPolygon()))
+    const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.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;
   }
   return std::nullopt;
@@ -64,18 +81,20 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
 /// @brief calculate the slowdown point to insert in the path
 /// @param ego_data ego data (path, 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 params parameters
 /// @return optional slowdown point to insert in the path
 std::optional<SlowdownToInsert> calculate_slowdown_point(
-  const EgoData & ego_data, const std::vector<Slowdown> & decisions, PlannerParam params)
+  const EgoData & ego_data, const std::vector<Slowdown> & decisions,
+  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params)
 {
   params.extra_front_offset += params.dist_buffer;
   const auto base_footprint = make_base_footprint(params);
 
   // 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, params);
+      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};
   }
   return std::nullopt;

planning/behavior_velocity_out_of_lane_module/src/debug.cpp

@@ -32,13 +32,12 @@ visualization_msgs::msg::Marker get_base_marker()
   base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
   base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5);
-  base_marker.lifetime = rclcpp::Duration::from_seconds(0.5);
   return base_marker;
 }
 }  // namespace
 void add_footprint_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
-  const lanelet::BasicPolygons2d & footprints, const double z)
+  const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = "footprints";
@@ -52,12 +51,14 @@ void add_footprint_markers(
     debug_marker.id++;
     debug_marker.points.clear();
   }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 void add_current_overlap_marker(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::BasicPolygon2d & current_footprint,
-  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z)
+  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = "current_overlap";
@@ -80,12 +81,14 @@ void add_current_overlap_marker(
     debug_marker_array.markers.push_back(debug_marker);
     debug_marker.id++;
   }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 void add_lanelet_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::ConstLanelets & lanelets, const std::string & ns,
-  const std_msgs::msg::ColorRGBA & color)
+  const std_msgs::msg::ColorRGBA & color, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = ns;
@@ -101,6 +104,82 @@ void add_lanelet_markers(
     debug_marker_array.markers.push_back(debug_marker);
     debug_marker.id++;
   }
+  debug_marker.action = debug_marker.DELETE;
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
+}
+
+void add_range_markers(
+  visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx,
+  const double z, const size_t prev_nb)
+{
+  auto debug_marker = get_base_marker();
+  debug_marker.ns = "ranges";
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5);
+  for (const auto & range : ranges) {
+    debug_marker.points.clear();
+    debug_marker.points.push_back(
+      path.points[first_ego_idx + range.entering_path_idx].point.pose.position);
+    debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+      range.entering_point.x(), range.entering_point.y(), z));
+    for (const auto & overlap : range.debug.overlaps) {
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z));
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z));
+    }
+    debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+      range.exiting_point.x(), range.exiting_point.y(), z));
+    debug_marker.points.push_back(
+      path.points[first_ego_idx + range.exiting_path_idx].point.pose.position);
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.id++;
+  }
+  debug_marker.action = debug_marker.DELETE;
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
+  debug_marker.action = debug_marker.ADD;
+  debug_marker.id = 0;
+  debug_marker.ns = "decisions";
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0);
+  debug_marker.points.clear();
+  for (const auto & range : ranges) {
+    debug_marker.type = debug_marker.LINE_STRIP;
+    if (range.debug.decision) {
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        range.entering_point.x(), range.entering_point.y(), z));
+      debug_marker.points.push_back(
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position);
+    }
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.points.clear();
+    debug_marker.id++;
+
+    debug_marker.type = debug_marker.TEXT_VIEW_FACING;
+    debug_marker.pose.position.x = range.entering_point.x();
+    debug_marker.pose.position.y = range.entering_point.y();
+    debug_marker.pose.position.z = z;
+    std::stringstream ss;
+    ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time
+       << "\n";
+    if (range.debug.object) {
+      debug_marker.pose.position.x +=
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x;
+      debug_marker.pose.position.y +=
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y;
+      debug_marker.pose.position.x /= 2;
+      debug_marker.pose.position.y /= 2;
+      ss << "Obj: " << range.debug.times.object.enter_time << " - "
+         << range.debug.times.object.exit_time << "\n";
+    }
+    debug_marker.scale.z = 1.0;
+    debug_marker.text = ss.str();
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.id++;
+  }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 }  // namespace behavior_velocity_planner::out_of_lane::debug