From 69466aad7b973362ec708aee65b2da365c619314 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 28 Sep 2023 13:24:02 +0900 Subject: [PATCH] feat(intersection): use planned velocity from upstream modules (#5156) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 ++ .../package.xml | 1 + .../src/manager.cpp | 4 ++++ .../src/scene_intersection.cpp | 13 +++++++----- .../src/scene_intersection.hpp | 4 +++- .../src/util.cpp | 20 ++++++++++++------- .../src/util.hpp | 5 +++-- 7 files changed, 34 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index def2335e20358..e1ae01e581f86 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -31,6 +31,8 @@ collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module + minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index eee6ca6ffb468..0ad01051746de 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -9,6 +9,7 @@ Takayuki Murooka Tomoya Kimura Shumpei Wakabayashi + Kyoichi Sugahara Apache License 2.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 83bc33f6b95bf..2813105766c2c 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -98,6 +98,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node, ns + ".collision_detection.relaxed.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); + ip.collision_detection.use_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); + ip.collision_detection.minimum_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c5fcaca40c24f..4e0b91df8336d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -875,7 +875,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, path_lanelets, closest_idx, time_delay, + std::min(occlusion_peeking_stop_line_idx, path->points.size() - 1), tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1060,7 +1061,7 @@ bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on) + const size_t last_intersection_stop_line_candidate_idx, const bool tl_arrow_solid_on) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1068,9 +1069,11 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, time_delay, - planner_param_.common.intersection_velocity, - planner_param_.collision_detection.minimum_ego_predicted_velocity); + path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, + time_delay, planner_param_.common.intersection_velocity, + planner_param_.collision_detection.minimum_ego_predicted_velocity, + planner_param_.collision_detection.use_upstream_velocity, + planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6684ae5ae0b45..e75b22dbe828a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -92,6 +92,8 @@ class IntersectionModule : public SceneModuleInterface double collision_end_margin_time; } relaxed; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr + bool use_upstream_velocity; + double minimum_upstream_velocity; } collision_detection; struct Occlusion { @@ -261,7 +263,7 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on); + const size_t last_intersection_stop_line_candidate_idx, const bool tl_arrow_solid_on); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 6dd7b7022a1d8..7767949c54242 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1012,8 +1012,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity) + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1023,7 +1024,9 @@ TimeDistanceArray calcIntersectionPassingTime( PathWithLaneId reference_path; for (size_t i = closest_idx; i < path.points.size(); ++i) { auto reference_point = path.points.at(i); - reference_point.point.longitudinal_velocity_mps = intersection_velocity; + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } reference_path.points.push_back(reference_point); bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids); if (assigned_lane_found && !has_objective_lane_id) { @@ -1054,10 +1057,13 @@ TimeDistanceArray calcIntersectionPassingTime( // use average velocity between p1 and p2 const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - passing_time += - (dist / std::max( - minimum_ego_velocity, - average_velocity)); // to avoid zero-division + const double minimum_ego_velocity_division = + (use_upstream_velocity && i > last_intersection_stop_line_candidate_idx) + ? minimum_upstream_velocity /* to avoid null division */ + : minimum_ego_velocity; + const double passing_velocity = + std::max(minimum_ego_velocity_division, average_velocity); + passing_time += (dist / passing_velocity); time_distance_array.emplace_back(passing_time, dist_sum); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index c3dd99a5a08ad..4b9cf9caedee8 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -148,8 +148,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity); + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet,