From e0d53ceab26cf441822f58961ce7f8b3e576dac4 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sun, 26 Nov 2023 23:25:45 +0900 Subject: [PATCH 1/3] feat(run_out): use the predicted object's velocity and covariance for the collision detection (#5672) * feat(run_out): use the predicted object's velocity and covariance for the collision detection Signed-off-by: Tomohito Ando * chore: update readme Signed-off-by: Tomohito Ando * fix calculation Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- .../README.md | 20 ++++++----- .../config/run_out.param.yaml | 19 +++++----- .../src/dynamic_obstacle.cpp | 36 +++++++++++++++++-- .../src/manager.cpp | 9 +++-- .../src/utils.hpp | 2 ++ 5 files changed, 64 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index a36cfdf6485c6..c48f2a951cd55 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -188,15 +188,17 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `margin_ahead` | double | [m] ahead margin for detection area polygon | | `margin_behind` | double | [m] behind margin for detection area polygon | -| Parameter /dynamic_obstacle | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | -| `min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles | -| `max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles | -| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points | -| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points | -| `max_prediction_time` | double | [sec] create predicted path until this time | -| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path | -| `points_interval` | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method | +| Parameter /dynamic_obstacle | Type | Description | +| ------------------------------------ | ------ | ----------------------------------------------------------------------------------------------------------------------------- | +| `use_mandatory_area` | double | [-] whether to use mandatory detection area | +| `assume_fixed_velocity.enable` | double | [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below | +| `assume_fixed_velocity.min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles | +| `assume_fixed_velocity.max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles | +| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points | +| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points | +| `max_prediction_time` | double | [sec] create predicted path until this time | +| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path | +| `points_interval` | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method | | Parameter /approaching | Type | Description | | ---------------------- | ------ | ----------------------------------------------------- | 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 2641214ac5ad4..ea63462b32d84 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 @@ -21,14 +21,17 @@ # parameter to create abstracted dynamic obstacles dynamic_obstacle: - use_mandatory_area: false # [-] whether to use mandatory detection area - min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles - max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles - diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points - height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points - max_prediction_time: 10.0 # [sec] create predicted path until this time - time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path - points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method + use_mandatory_area: false # [-] whether to use mandatory detection area + assume_fixed_velocity: + enable: false # [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below + min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles + max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles + std_dev_multiplier: 1.96 # [-] min and max velocity of the obstacles are calculated from this value and covariance + diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points + height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points + max_prediction_time: 10.0 # [sec] create predicted path until this time + time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method # approach if ego has stopped in the front of the obstacle for a certain amount of time approaching: diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index e2f9880435d5e..a1b53d6adc966 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -278,6 +278,31 @@ PointCloud2 concatPointCloud( return concat_points; } +void calculateMinAndMaxVelFromCovariance( + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double std_dev_multiplier, run_out_utils::DynamicObstacle & dynamic_obstacle) +{ + const double x_velocity = std::abs(twist_with_covariance.twist.linear.x); + const double y_velocity = std::abs(twist_with_covariance.twist.linear.y); + const double x_variance = twist_with_covariance.covariance.at(0); + const double y_variance = twist_with_covariance.covariance.at(7); + const double x_std_dev = std::sqrt(x_variance); + const double y_std_dev = std::sqrt(y_variance); + + // calculate the min and max velocity using the standard deviation of twist + // note that this assumes the covariance of x and y is zero + const double min_x = std::max(0.0, x_velocity - std_dev_multiplier * x_std_dev); + const double min_y = std::max(0.0, y_velocity - std_dev_multiplier * y_std_dev); + const double min_velocity = std::hypot(min_x, min_y); + + const double max_x = x_velocity + std_dev_multiplier * x_std_dev; + const double max_y = y_velocity + std_dev_multiplier * y_std_dev; + const double max_velocity = std::hypot(max_x, max_y); + + dynamic_obstacle.min_velocity_mps = min_velocity; + dynamic_obstacle.max_velocity_mps = max_velocity; +} + } // namespace DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject( @@ -294,9 +319,14 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta DynamicObstacle dynamic_obstacle; dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose; - // TODO(Tomohito Ando): calculate velocity from covariance of predicted_object - dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + if (param_.assume_fixed_velocity) { + dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + } else { + calculateMinAndMaxVelFromCovariance( + predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier, + dynamic_obstacle); + } dynamic_obstacle.classifications = predicted_object.classification; dynamic_obstacle.shape = predicted_object.shape; diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index a5253f59b60f9..09c87ed81cf38 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -84,8 +84,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) auto & p = planner_param_.dynamic_obstacle; const std::string ns_do = ns + ".dynamic_obstacle"; p.use_mandatory_area = getOrDeclareParameter(node, ns_do + ".use_mandatory_area"); - p.min_vel_kmph = getOrDeclareParameter(node, ns_do + ".min_vel_kmph"); - p.max_vel_kmph = getOrDeclareParameter(node, ns_do + ".max_vel_kmph"); + p.assume_fixed_velocity = + getOrDeclareParameter(node, ns_do + ".assume_fixed_velocity.enable"); + p.min_vel_kmph = + getOrDeclareParameter(node, ns_do + ".assume_fixed_velocity.min_vel_kmph"); + p.max_vel_kmph = + getOrDeclareParameter(node, ns_do + ".assume_fixed_velocity.max_vel_kmph"); + p.std_dev_multiplier = getOrDeclareParameter(node, ns_do + ".std_dev_multiplier"); p.diameter = getOrDeclareParameter(node, ns_do + ".diameter"); p.height = getOrDeclareParameter(node, ns_do + ".height"); p.max_prediction_time = getOrDeclareParameter(node, ns_do + ".max_prediction_time"); diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 9b746589d3f93..79555444f023a 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -109,11 +109,13 @@ struct Smoother struct DynamicObstacleParam { bool use_mandatory_area; + bool assume_fixed_velocity; float min_vel_kmph; float max_vel_kmph; // parameter to convert points to dynamic obstacle + float std_dev_multiplier; float diameter; // [m] float height; // [m] float max_prediction_time; // [sec] From 859885e1740fdb6d3a847303364b052471a01069 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 15 Dec 2023 23:45:24 +0900 Subject: [PATCH 2/3] perf(run_out): improve calculation cost of smoothPath (#5885) * perf(run_out): improve calculation cost of smoothPath Signed-off-by: Takayuki Murooka * re-index enum elements Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/debug.hpp | 19 +++--- .../src/scene.cpp | 68 ++++++++++++------- 2 files changed, 54 insertions(+), 33 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 273abb10c780a..0a2a92f3055fb 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -33,14 +33,17 @@ class DebugValues public: enum class TYPE { CALCULATION_TIME = 0, - CALCULATION_TIME_COLLISION_CHECK = 1, - LATERAL_DIST = 2, - LONGITUDINAL_DIST_OBSTACLE = 3, - LONGITUDINAL_DIST_COLLISION = 4, - COLLISION_POS_FROM_EGO_FRONT = 5, - STOP_DISTANCE = 6, - NUM_OBSTACLES = 7, - LATERAL_PASS_DIST = 8, + CALCULATION_TIME_PATH_PROCESSING = 1, + CALCULATION_TIME_OBSTACLE_CREATION = 2, + CALCULATION_TIME_COLLISION_CHECK = 3, + CALCULATION_TIME_PATH_PLANNING = 4, + LATERAL_DIST = 5, + LONGITUDINAL_DIST_OBSTACLE = 6, + LONGITUDINAL_DIST_COLLISION = 7, + COLLISION_POS_FROM_EGO_FRONT = 8, + STOP_DISTANCE = 9, + NUM_OBSTACLES = 10, + LATERAL_PASS_DIST = 11, SIZE, // this is the number of enum elements }; diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index a943e37d666db..0cf7714cbcaee 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -55,7 +55,7 @@ bool RunOutModule::modifyPathVelocity( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { // timer starts - const auto t1_modify_path = std::chrono::system_clock::now(); + const auto t_start = std::chrono::system_clock::now(); // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; @@ -65,20 +65,27 @@ bool RunOutModule::modifyPathVelocity( // set height of debug data debug_ptr_->setHeight(current_pose.position.z); - // smooth velocity of the path to calculate time to collision accurately - PathWithLaneId smoothed_path; - if (!smoothPath(*path, smoothed_path, planner_data_)) { - return true; - } - // extend path to consider obstacles after the goal - const auto extended_smoothed_path = - run_out_utils::extendPath(smoothed_path, planner_param_.vehicle_param.base_to_front); + const auto extended_path = + run_out_utils::extendPath(*path, planner_param_.vehicle_param.base_to_front); // trim path ahead of the base_link to make calculation easier const double trim_distance = planner_param_.run_out.detection_distance; - const auto trim_smoothed_path = - run_out_utils::trimPathFromSelfPose(extended_smoothed_path, current_pose, trim_distance); + const auto trim_path = + run_out_utils::trimPathFromSelfPose(extended_path, current_pose, trim_distance); + + // smooth velocity of the path to calculate time to collision accurately + PathWithLaneId extended_smoothed_path; + if (!smoothPath(trim_path, extended_smoothed_path, planner_data_)) { + return true; + } + + // record time for path processing + const auto t_path_processing = std::chrono::system_clock::now(); + const auto elapsed_path_processing = + std::chrono::duration_cast(t_path_processing - t_start); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_PATH_PROCESSING, elapsed_path_processing.count() / 1000.0); // create abstracted dynamic obstacles from objects or points dynamic_obstacle_creator_->setData(*planner_data_, planner_param_, *path, extended_smoothed_path); @@ -87,18 +94,24 @@ bool RunOutModule::modifyPathVelocity( // extract obstacles using lanelet information const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, trim_smoothed_path, current_pose); + excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); - // timer starts - const auto t1_collision_check = std::chrono::system_clock::now(); + // record time for obstacle creation + const auto t_obstacle_creation = std::chrono::system_clock::now(); + const auto elapsed_obstacle_creation = + std::chrono::duration_cast(t_obstacle_creation - t_path_processing); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_OBSTACLE_CREATION, + elapsed_obstacle_creation.count() / 1000.0); // detect collision with dynamic obstacles using velocity planning of ego - const auto dynamic_obstacle = detectCollision(partition_excluded_obstacles, trim_smoothed_path); + const auto dynamic_obstacle = + detectCollision(partition_excluded_obstacles, extended_smoothed_path); - // timer ends - const auto t2_collision_check = std::chrono::system_clock::now(); + // record time for collision check + const auto t_collision_check = std::chrono::system_clock::now(); const auto elapsed_collision_check = - std::chrono::duration_cast(t2_collision_check - t1_collision_check); + std::chrono::duration_cast(t_collision_check - t_obstacle_creation); debug_ptr_->setDebugValues( DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); @@ -107,7 +120,7 @@ bool RunOutModule::modifyPathVelocity( // after a certain amount of time has elapsed since the ego stopped, // approach the obstacle with slow velocity insertVelocityForState( - dynamic_obstacle, *planner_data_, planner_param_, trim_smoothed_path, *path); + dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path); } else { // just insert zero velocity for stopping insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); @@ -119,14 +132,19 @@ bool RunOutModule::modifyPathVelocity( } publishDebugValue( - trim_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); - // timer ends - const auto t2_modify_path = std::chrono::system_clock::now(); - const auto elapsed_modify_path = - std::chrono::duration_cast(t2_modify_path - t1_modify_path); + // record time for collision check + const auto t_path_planning = std::chrono::system_clock::now(); + const auto elapsed_path_planning = + std::chrono::duration_cast(t_path_planning - t_collision_check); debug_ptr_->setDebugValues( - DebugValues::TYPE::CALCULATION_TIME, elapsed_modify_path.count() / 1000.0); + DebugValues::TYPE::CALCULATION_TIME_PATH_PLANNING, elapsed_path_planning.count() / 1000.0); + + // record time for the function + const auto t_end = std::chrono::system_clock::now(); + const auto elapsed_all = std::chrono::duration_cast(t_end - t_start); + debug_ptr_->setDebugValues(DebugValues::TYPE::CALCULATION_TIME, elapsed_all.count() / 1000.0); return true; } From f14f5ee8fff0b389c6b3639057c0dcebefa55fb4 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 11 Apr 2024 15:52:32 +0900 Subject: [PATCH 3/3] feat(run_out)!: ignore the collision points on crosswalk (#5862) * suppress on crosswalk Signed-off-by: Yuki Takagi Signed-off-by: Tomohito Ando --- .../config/run_out.param.yaml | 1 + .../package.xml | 1 + .../src/manager.cpp | 1 + .../src/scene.cpp | 48 ++++++++++++++++--- .../src/scene.hpp | 10 ++-- .../src/utils.hpp | 1 + 6 files changed, 53 insertions(+), 9 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 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;