From 41f3ae180f7cd3ab631a8e3b2bb88d6b4910470d Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 18 Aug 2022 19:44:15 +0900 Subject: [PATCH] feat(behavior_velocity): add insertDecel point function (#1615) Signed-off-by: tanaka3 Signed-off-by: tanaka3 --- .../crosswalk/scene_crosswalk.hpp | 2 +- .../include/utilization/util.hpp | 4 +++ .../crosswalk/scene_crosswalk.cpp | 30 +++++++------------ .../risk_predictive_braking.cpp | 2 +- .../src/utilization/util.cpp | 22 ++++++++++++++ 5 files changed, 39 insertions(+), 21 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index 30521c3fd0b91..165fd6e4993e8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -120,7 +120,7 @@ class CrosswalkModule : public SceneModuleInterface std::pair getAttentionRange(const PathWithLaneId & ego_path); - void insertDecelPoint( + void insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, PathWithLaneId & output); diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index de7f0406e6145..69dd2e9d548b5 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -347,6 +347,10 @@ std::set getLaneletIdSetOnPath( return id_set; } +boost::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity); + std::vector getLaneletsOnPath( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 4451a67f72221..1d46c90b14f50 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -200,7 +200,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } const auto target_velocity = calcTargetVelocity(nearest_stop_point.get(), ego_path); - insertDecelPoint( + insertDecelPointWithDebugInfo( nearest_stop_point.get(), std::max(planner_param_.min_slow_down_velocity, target_velocity), *path); @@ -208,10 +208,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } if (nearest_stop_point) { - insertDecelPoint(nearest_stop_point.get(), 0.0, *path); + insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor, stop_reason); } else if (rtc_stop_point) { - insertDecelPoint(rtc_stop_point.get(), 0.0, *path); + insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor_rtc, stop_reason); } @@ -430,32 +430,24 @@ std::pair CrosswalkModule::getAttentionRange(const PathWithLaneI return std::make_pair(std::max(0.0, near_attention_range), std::max(0.0, far_attention_range)); } -void CrosswalkModule::insertDecelPoint( +void CrosswalkModule::insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, PathWithLaneId & output) { - const auto & ego_pos = planner_data_->current_pose.pose.position; - const size_t base_idx = findNearestSegmentIndex(output.points, stop_point); - const auto insert_idx = insertTargetPoint(base_idx, stop_point, output.points); - - if (!insert_idx) { + const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity); + if (!stop_pose) { return; } + const auto & ego_pos = planner_data_->current_pose.pose.position; - for (size_t i = insert_idx.get(); i < output.points.size(); ++i) { - const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps; - output.points.at(i).point.longitudinal_velocity_mps = - std::min(original_velocity, target_velocity); - } - - setDistance(calcSignedArcLength(output.points, ego_pos, stop_point)); + setDistance(calcSignedArcLength(output.points, ego_pos, stop_pose->position)); - debug_data_.first_stop_pose = getPose(output.points.at(insert_idx.get())); + debug_data_.first_stop_pose = getPose(*stop_pose); if (std::abs(target_velocity) < 1e-3) { - debug_data_.stop_poses.push_back(getPose(output.points.at(insert_idx.get()))); + debug_data_.stop_poses.push_back(*stop_pose); } else { - debug_data_.slow_poses.push_back(getPose(output.points.at(insert_idx.get()))); + debug_data_.slow_poses.push_back(*stop_pose); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 319589252f71f..e5cd038bb7dd4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -60,7 +60,7 @@ void applySafeVelocityConsideringPossibleCollision( safe_velocity = std::max(safe_velocity, v_min); possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity; const auto & pose = possible_collision.collision_with_margin.pose; - insertSafeVelocityToPath(pose, safe_velocity, param, inout_path); + planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); } } diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index c83640b01c799..a3e88500ec8d5 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -246,6 +246,28 @@ void insertVelocity( setVelocityFromIndex(insert_index, v, &path); } +boost::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity) +{ + // TODO(tanaka): consider proper overlap threshold for inserting decel point + const double overlap_threshold = 5e-2; + const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = + motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.get(); i < output.points.size(); ++i) { + const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps; + output.points.at(i).point.longitudinal_velocity_mps = + std::min(original_velocity, target_velocity); + } + return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); +} + Polygon2d toFootprintPolygon(const PredictedObject & object) { Polygon2d obj_footprint;