From c2abb5cf11ed5ea5472053ae4154062222e30b05 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 31 Oct 2024 20:42:34 +0900 Subject: [PATCH] feat(obstacle_cruise_planner)!: relax unknown stop (#1612) * chore: fix dependency of mult object tracker (#1567) fix dependency of mult object tracker * add relax logic Signed-off-by: Yuki Takagi * define params Signed-off-by: Yuki Takagi * add universe params Signed-off-by: Yuki Takagi * delete null line Signed-off-by: Yuki Takagi * fix spell Signed-off-by: Yuki Takagi * fix Signed-off-by: Yuki Takagi * change universe params Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi Co-authored-by: Shohei Sakai --- .../config/obstacle_cruise_planner.param.yaml | 2 ++ .../include/obstacle_cruise_planner/node.hpp | 2 ++ .../obstacle_cruise_planner/planner_interface.hpp | 7 ++++++- planning/obstacle_cruise_planner/src/node.cpp | 12 ++++++++++-- .../src/planner_interface.cpp | 13 +++++++++++++ 5 files changed, 33 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index fd97d829b8188..b14593ae1819c 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -222,3 +222,5 @@ sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. + limit_margin_for_unknown: 6.0 + preferred_acc_for_unknown: -6.0 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 909681255d4e9..a879e90b6be87 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -110,6 +110,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double additional_safe_distance_margin_on_curve_; double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; + double limit_margin_for_unknown_; + double preferred_acc_for_unknown_; std::vector stop_obstacle_types_; std::vector inside_cruise_obstacle_types_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 7a060657e16af..d79780a6ee66b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -65,7 +65,8 @@ class PlannerInterface const bool enable_debug_info, const bool enable_calculation_time_info, const double min_behavior_stop_margin, const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve, - const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop, + const double limit_margin_for_unknown, const double preferred_acc_for_unknown) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; @@ -74,6 +75,8 @@ class PlannerInterface additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; + limit_margin_for_unknown_ = limit_margin_for_unknown; + preferred_acc_for_unknown_ = preferred_acc_for_unknown; } std::vector generateStopTrajectory( @@ -123,6 +126,8 @@ class PlannerInterface double additional_safe_distance_margin_on_curve_; double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; + double limit_margin_for_unknown_; + double preferred_acc_for_unknown_; // stop watch tier4_autoware_utils::StopWatch< diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 5d924452e8172..c1f47bdb7b656 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -445,10 +445,13 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & declare_parameter("common.stop_on_curve.min_safe_distance_margin"); suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); + limit_margin_for_unknown_ = declare_parameter("stop.limit_margin_for_unknown"); + preferred_acc_for_unknown_ = declare_parameter("stop.preferred_acc_for_unknown"); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, - min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_, + preferred_acc_for_unknown_); } { // stop/cruise/slow down obstacle type @@ -496,11 +499,16 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( tier4_autoware_utils::updateParam( parameters, "common.stop_on_curve.min_safe_distance_margin", min_safe_distance_margin_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "stop.limit_margin_for_unknown", limit_margin_for_unknown_); + tier4_autoware_utils::updateParam( + parameters, "stop.preferred_acc_for_unknown", preferred_acc_for_unknown_); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, - min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_, + preferred_acc_for_unknown_); tier4_autoware_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index c7ac3c3c63a40..1d1a39d0af282 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -300,6 +300,19 @@ std::vector PlannerInterface::generateStopTrajectory( // calc stop point against the obstacle double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); + if (stop_obstacle.classification.label == ObjectClassification::UNKNOWN) { + const double pref_acc_stop_dist = + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, preferred_acc_for_unknown_); + const double limit_margin_stop_dist = + std::max(0.0, dist_to_collide_on_ref_traj - limit_margin_for_unknown_); + if (pref_acc_stop_dist > candidate_zero_vel_dist) { + candidate_zero_vel_dist = std::min(pref_acc_stop_dist, limit_margin_stop_dist); + } + } + if (suppress_sudden_obstacle_stop_) { const auto acceptable_stop_acc = [&]() -> std::optional { if (stop_param_.getParamType(stop_obstacle.classification) == "default") {