fix(behavior_velocity_run_out): construct predicted path up to max pr… (autowarefoundation#6650)

* fix(behavior_velocity_run_out): construct predicted path up to max prediction time in object method

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* handle division by zero

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add missing include

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

---------

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

Author: yuki-takagi-66

planning/behavior_velocity_run_out_module/config/run_out.param.yaml

@@ -3,6 +3,7 @@
     run_out:
       detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
       use_partition_lanelet: true # [-] whether to use partition lanelet map data
+      suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
       specify_decel_jerk: false   # [-] whether to specify jerk when ego decelerates
       stop_margin: 2.5            # [m] the vehicle decelerates to be able to stop with this margin
       passing_margin: 1.1         # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin

planning/behavior_velocity_run_out_module/package.xml

@@ -19,6 +19,7 @@
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>behavior_velocity_crosswalk_module</depend>
   <depend>behavior_velocity_planner_common</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>

planning/behavior_velocity_run_out_module/src/manager.cpp

@@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     auto & p = planner_param_.run_out;
     p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
     p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
+    p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");
     p.specify_decel_jerk = getOrDeclareParameter<bool>(node, ns + ".specify_decel_jerk");
     p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
     p.passing_margin = getOrDeclareParameter<double>(node, ns + ".passing_margin");

planning/behavior_velocity_run_out_module/src/scene.cpp

@@ -14,10 +14,20 @@
 
 #include "scene.hpp"
 
+#include "behavior_velocity_crosswalk_module/util.hpp"
 #include "path_utils.hpp"
 
 #include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <motion_utils/distance/distance.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <boost/geometry/algorithms/intersection.hpp>
+#include <boost/geometry/algorithms/within.hpp>
+
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_core/geometry/Polygon.h>
 
 #include <algorithm>
 #include <limits>
@@ -105,8 +115,14 @@ bool RunOutModule::modifyPathVelocity(
     elapsed_obstacle_creation.count() / 1000.0);
 
   // detect collision with dynamic obstacles using velocity planning of ego
+  const auto crosswalk_lanelets = planner_param_.run_out.suppress_on_crosswalk
+                                    ? getCrosswalksOnPath(
+                                        planner_data_->current_odometry->pose, *path,
+                                        planner_data_->route_handler_->getLaneletMapPtr(),
+                                        planner_data_->route_handler_->getOverallGraphPtr())
+                                    : std::vector<lanelet::ConstLanelet>();
   const auto dynamic_obstacle =
-    detectCollision(partition_excluded_obstacles, extended_smoothed_path);
+    detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets);
 
   // record time for collision check
   const auto t_collision_check = std::chrono::system_clock::now();
@@ -150,7 +166,8 @@ bool RunOutModule::modifyPathVelocity(
 }
 
 boost::optional<DynamicObstacle> RunOutModule::detectCollision(
-  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path)
+  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
+  const std::vector<lanelet::ConstLanelet> & crosswalk_lanelets)
 {
   if (path.points.size() < 2) {
     RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points.");
@@ -184,7 +201,7 @@ boost::optional<DynamicObstacle> RunOutModule::detectCollision(
     debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0);
 
     auto obstacles_collision =
-      checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time);
+      checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets);
     if (obstacles_collision.empty()) {
       continue;
     }
@@ -304,7 +321,8 @@ std::vector<geometry_msgs::msg::Point> RunOutModule::createVehiclePolygon(
 
 std::vector<DynamicObstacle> RunOutModule::checkCollisionWithObstacles(
   const std::vector<DynamicObstacle> & dynamic_obstacles,
-  std::vector<geometry_msgs::msg::Point> poly, const float travel_time) const
+  std::vector<geometry_msgs::msg::Point> poly, const float travel_time,
+  const std::vector<lanelet::ConstLanelet> & crosswalk_lanelets) const
 {
   const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly);
 
@@ -333,8 +351,8 @@ std::vector<DynamicObstacle> RunOutModule::checkCollisionWithObstacles(
       *predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel};
 
     std::vector<geometry_msgs::msg::Point> collision_points;
-    const bool collision_detected =
-      checkCollisionWithShape(bg_poly_vehicle, pose_with_range, obstacle.shape, collision_points);
+    const bool collision_detected = checkCollisionWithShape(
+      bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points);
 
     if (!collision_detected) {
       continue;
@@ -393,6 +411,7 @@ boost::optional<geometry_msgs::msg::Pose> RunOutModule::calcPredictedObstaclePos
 
 bool RunOutModule::checkCollisionWithShape(
   const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
+  const std::vector<lanelet::ConstLanelet> & crosswalk_lanelets,
   std::vector<geometry_msgs::msg::Point> & collision_points) const
 {
   bool collision_detected = false;
@@ -415,6 +434,23 @@ bool RunOutModule::checkCollisionWithShape(
       break;
   }
 
+  if (!collision_points.empty()) {
+    for (const auto & crosswalk : crosswalk_lanelets) {
+      const auto poly = crosswalk.polygon2d().basicPolygon();
+      for (auto it = collision_points.begin(); it != collision_points.end();) {
+        if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) {
+          it = collision_points.erase(it);
+        } else {
+          ++it;
+        }
+      }
+      if (collision_points.empty()) {
+        break;
+      }
+    }
+    collision_detected = !collision_points.empty();
+  }
+
   return collision_detected;
 }
 

planning/behavior_velocity_run_out_module/src/scene.hpp

@@ -23,7 +23,8 @@
 #include <behavior_velocity_planner_common/scene_module_interface.hpp>
 
 #include <memory>
-#include <string>
+#include <optional>
+#include <utility>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -68,7 +69,8 @@ class RunOutModule : public SceneModuleInterface
   Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;
 
   boost::optional<DynamicObstacle> detectCollision(
-    const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path_points);
+    const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
+    const std::vector<lanelet::ConstLanelet> & crosswalk_lanelets);
 
   float calcCollisionPositionOfVehicleSide(
     const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const;
@@ -78,7 +80,8 @@ class RunOutModule : public SceneModuleInterface
 
   std::vector<DynamicObstacle> checkCollisionWithObstacles(
     const std::vector<DynamicObstacle> & dynamic_obstacles,
-    std::vector<geometry_msgs::msg::Point> poly, const float travel_time) const;
+    std::vector<geometry_msgs::msg::Point> poly, const float travel_time,
+    const std::vector<lanelet::ConstLanelet> & crosswalk_lanelets) const;
 
   boost::optional<DynamicObstacle> findNearestCollisionObstacle(
     const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose,
@@ -90,6 +93,7 @@ class RunOutModule : public SceneModuleInterface
 
   bool checkCollisionWithShape(
     const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
+    const std::vector<lanelet::ConstLanelet> & crosswalk_lanelets,
     std::vector<geometry_msgs::msg::Point> & collision_points) const;
 
   bool checkCollisionWithCylinder(

planning/behavior_velocity_run_out_module/src/utils.hpp

@@ -49,6 +49,7 @@ struct RunOutParam
 {
   std::string detection_method;
   bool use_partition_lanelet;
+  bool suppress_on_crosswalk;
   bool specify_decel_jerk;
   double stop_margin;
   double passing_margin;