From db591c1f86adae68413675f09e9a07959d237cb4 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 13 Dec 2023 20:53:56 +0900 Subject: [PATCH 1/2] suppress on crosswalk Signed-off-by: Yuki Takagi --- .../config/run_out.param.yaml | 1 + .../package.xml | 1 + .../src/manager.cpp | 1 + .../src/scene.cpp | 42 +++++++++++++++---- .../src/scene.hpp | 8 ++-- .../src/utils.hpp | 1 + 6 files changed, 44 insertions(+), 10 deletions(-) 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 7bd27fca3407c..809579b383461 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -20,6 +20,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 7a13c0c4f1052..a3b64b6d1c4ae 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -14,6 +14,7 @@ #include "scene.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" #include @@ -23,6 +24,10 @@ #include #include +#include + +#include +#include #include #include @@ -111,7 +116,7 @@ bool RunOutModule::modifyPathVelocity( // detect collision with dynamic obstacles using velocity planning of ego const auto dynamic_obstacle = - detectCollision(partition_excluded_obstacles, extended_smoothed_path); + detectCollision(partition_excluded_obstacles, extended_smoothed_path, *path); // record time for collision check const auto t_collision_check = std::chrono::system_clock::now(); @@ -155,7 +160,8 @@ bool RunOutModule::modifyPathVelocity( } std::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const PathWithLaneId & raw_path) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -189,7 +195,7 @@ std::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, raw_path); if (obstacles_collision.empty()) { continue; } @@ -309,7 +315,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, + PathWithLaneId raw_path) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -338,8 +345,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, raw_path, collision_points); if (!collision_detected) { continue; @@ -398,7 +405,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( bool RunOutModule::checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, - std::vector & collision_points) const + const PathWithLaneId raw_path, std::vector & collision_points) const { bool collision_detected = false; switch (shape.type) { @@ -420,6 +427,27 @@ bool RunOutModule::checkCollisionWithShape( break; } + if (!collision_points.empty() && planner_param_.run_out.suppress_on_crosswalk) { + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, raw_path, + planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->route_handler_->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + const auto poly = crosswalk.second.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 b1c49189267d4..f9ee815874a49 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -67,7 +67,8 @@ class RunOutModule : public SceneModuleInterface Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; std::optional detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const PathWithLaneId & raw_path); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -77,7 +78,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, + PathWithLaneId path) const; std::optional findNearestCollisionObstacle( const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, @@ -89,7 +91,7 @@ class RunOutModule : public SceneModuleInterface bool checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, - std::vector & collision_points) const; + const PathWithLaneId path, std::vector & collision_points) const; bool checkCollisionWithCylinder( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius, diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 6afe451f72f73..5524c0f76049d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -54,6 +54,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; From 14cdb34f33f6fccb3a0ab033a3a443fe4b28f232 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 18 Dec 2023 20:04:29 +0900 Subject: [PATCH 2/2] update Signed-off-by: Yuki Takagi --- .../src/scene.cpp | 29 ++++++++++--------- .../src/scene.hpp | 8 +++-- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index a3b64b6d1c4ae..92516e7b4424b 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -115,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, *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(); @@ -161,7 +167,7 @@ bool RunOutModule::modifyPathVelocity( std::optional RunOutModule::detectCollision( const std::vector & dynamic_obstacles, const PathWithLaneId & path, - const PathWithLaneId & raw_path) + const std::vector> & crosswalk_lanelets) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -195,7 +201,7 @@ std::optional RunOutModule::detectCollision( debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0); auto obstacles_collision = - checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, raw_path); + checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets); if (obstacles_collision.empty()) { continue; } @@ -316,7 +322,7 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, std::vector poly, const float travel_time, - PathWithLaneId raw_path) const + const std::vector> & crosswalk_lanelets) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -346,7 +352,7 @@ std::vector RunOutModule::checkCollisionWithObstacles( std::vector collision_points; const bool collision_detected = checkCollisionWithShape( - bg_poly_vehicle, pose_with_range, obstacle.shape, raw_path, collision_points); + bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points); if (!collision_detected) { continue; @@ -405,7 +411,8 @@ std::optional RunOutModule::calcPredictedObstaclePose( bool RunOutModule::checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, - const PathWithLaneId raw_path, std::vector & collision_points) const + const std::vector> & crosswalk_lanelets, + std::vector & collision_points) const { bool collision_detected = false; switch (shape.type) { @@ -427,16 +434,11 @@ bool RunOutModule::checkCollisionWithShape( break; } - if (!collision_points.empty() && planner_param_.run_out.suppress_on_crosswalk) { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, raw_path, - planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->route_handler_->getOverallGraphPtr()); - + if (!collision_points.empty()) { for (const auto & crosswalk : crosswalk_lanelets) { const auto poly = crosswalk.second.polygon2d().basicPolygon(); for (auto it = collision_points.begin(); it != collision_points.end();) { - if (boost::geometry::within(lanelet::BasicPoint2d((*it).x, (*it).y), poly)) { + if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) { it = collision_points.erase(it); } else { ++it; @@ -448,6 +450,7 @@ bool RunOutModule::checkCollisionWithShape( } 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 f9ee815874a49..def90f036c440 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -68,7 +69,7 @@ class RunOutModule : public SceneModuleInterface std::optional detectCollision( const std::vector & dynamic_obstacles, const PathWithLaneId & path, - const PathWithLaneId & raw_path); + const std::vector> & crosswalk_lanelets); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -79,7 +80,7 @@ class RunOutModule : public SceneModuleInterface std::vector checkCollisionWithObstacles( const std::vector & dynamic_obstacles, std::vector poly, const float travel_time, - PathWithLaneId path) const; + const std::vector> & crosswalk_lanelets) const; std::optional findNearestCollisionObstacle( const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, @@ -91,7 +92,8 @@ class RunOutModule : public SceneModuleInterface bool checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, - const PathWithLaneId path, std::vector & collision_points) const; + const std::vector> & crosswalk_lanelets, + std::vector & collision_points) const; bool checkCollisionWithCylinder( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius,