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;