diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml
index ea63462b32d84..5534228c1b86f 100644
--- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml
+++ b/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
diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml
index fb3c49bb750a6..0acfc55a400b5 100644
--- a/planning/behavior_velocity_run_out_module/package.xml
+++ b/planning/behavior_velocity_run_out_module/package.xml
@@ -19,6 +19,7 @@
autoware_auto_perception_msgs
autoware_auto_planning_msgs
+ behavior_velocity_crosswalk_module
behavior_velocity_planner_common
eigen
geometry_msgs
diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp
index 09c87ed81cf38..3ba9bf8bf52e6 100644
--- a/planning/behavior_velocity_run_out_module/src/manager.cpp
+++ b/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(node, ns + ".detection_method");
p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet");
+ p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk");
p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk");
p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin");
p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin");
diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp
index 0cf7714cbcaee..43fd767bfe70e 100644
--- a/planning/behavior_velocity_run_out_module/src/scene.cpp
+++ b/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
#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
#include
#include
@@ -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();
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 RunOutModule::detectCollision(
- const std::vector & dynamic_obstacles, const PathWithLaneId & path)
+ const std::vector & dynamic_obstacles, const PathWithLaneId & path,
+ const std::vector & crosswalk_lanelets)
{
if (path.points.size() < 2) {
RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points.");
@@ -184,7 +201,7 @@ boost::optional 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 RunOutModule::createVehiclePolygon(
std::vector RunOutModule::checkCollisionWithObstacles(
const std::vector & dynamic_obstacles,
- std::vector poly, const float travel_time) const
+ std::vector poly, const float travel_time,
+ const std::vector & crosswalk_lanelets) const
{
const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly);
@@ -333,8 +351,8 @@ std::vector RunOutModule::checkCollisionWithObstacles(
*predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel};
std::vector 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 RunOutModule::calcPredictedObstaclePos
bool RunOutModule::checkCollisionWithShape(
const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
+ const std::vector & crosswalk_lanelets,
std::vector & 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;
}
diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp
index 925d8ea8b234c..1f8debeb266d0 100644
--- a/planning/behavior_velocity_run_out_module/src/scene.hpp
+++ b/planning/behavior_velocity_run_out_module/src/scene.hpp
@@ -23,7 +23,8 @@
#include
#include
-#include
+#include
+#include
#include
namespace behavior_velocity_planner
@@ -68,7 +69,8 @@ class RunOutModule : public SceneModuleInterface
Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;
boost::optional detectCollision(
- const std::vector & dynamic_obstacles, const PathWithLaneId & path_points);
+ const std::vector & dynamic_obstacles, const PathWithLaneId & path,
+ const std::vector & 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 checkCollisionWithObstacles(
const std::vector & dynamic_obstacles,
- std::vector poly, const float travel_time) const;
+ std::vector poly, const float travel_time,
+ const std::vector & crosswalk_lanelets) const;
boost::optional 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 & crosswalk_lanelets,
std::vector & collision_points) const;
bool checkCollisionWithCylinder(
diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp
index 79555444f023a..9aa3e8bf53539 100644
--- a/planning/behavior_velocity_run_out_module/src/utils.hpp
+++ b/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;