From 45800ef68038d7104add11bcf6aa47b347fab4ef Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 1 Oct 2024 09:33:23 +0900 Subject: [PATCH 1/8] chore: fix dependency of mult object tracker (#1567) fix dependency of mult object tracker --- perception/multi_object_tracker/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index e3172dfd22349..992b1f5e2a123 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -13,6 +13,7 @@ eigen3_cmake_module autoware_auto_perception_msgs + diagnostic_updater eigen kalman_filter mussp From 01affa7454695e380cd6f4c4e29d6dee66187805 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 29 Oct 2024 18:19:59 +0900 Subject: [PATCH 2/8] add relax logic Signed-off-by: Yuki Takagi --- .../src/planner_interface.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index c7ac3c3c63a40..d2e53d8c64e38 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -263,6 +263,9 @@ std::vector PlannerInterface::generateStopTrajectory( std::optional determined_zero_vel_dist{}; std::optional determined_desired_margin{}; + const double limit_margin_for_unknown = 3.0; + const double preffered_acc_for_unknown = -3.0; + const auto closest_stop_obstacles = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); for (const auto & stop_obstacle : closest_stop_obstacles) { @@ -300,6 +303,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, preffered_acc_for_unknown); + const double liimit_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, liimit_margin_stop_dist); + } + } + if (suppress_sudden_obstacle_stop_) { const auto acceptable_stop_acc = [&]() -> std::optional { if (stop_param_.getParamType(stop_obstacle.classification) == "default") { @@ -310,6 +326,7 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.ego_vel, longitudinal_info_.limit_max_accel, stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { return longitudinal_info_.limit_min_accel; } From f3a7f462f30a6ed38fd8198fa9ac0c2659d789ae Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 29 Oct 2024 19:29:56 +0900 Subject: [PATCH 3/8] define params Signed-off-by: Yuki Takagi --- .../include/obstacle_cruise_planner/node.hpp | 2 ++ .../obstacle_cruise_planner/planner_interface.hpp | 7 ++++++- planning/obstacle_cruise_planner/src/node.cpp | 13 +++++++++++-- .../src/planner_interface.cpp | 7 ++----- 4 files changed, 21 insertions(+), 8 deletions(-) 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..42d0b73a2d8c9 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 preffered_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..06f74b1f94b7b 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 preffered_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; + preffered_acc_for_unknown_ = preffered_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 preffered_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..6deaaaa2e26c0 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -445,10 +445,14 @@ 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"); + preffered_acc_for_unknown_ = declare_parameter("stop.preffered_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_, + preffered_acc_for_unknown_); + } { // stop/cruise/slow down obstacle type @@ -496,11 +500,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.preffered_acc_for_unknown", preffered_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_, + preffered_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 d2e53d8c64e38..909e661ff7c0d 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -263,9 +263,6 @@ std::vector PlannerInterface::generateStopTrajectory( std::optional determined_zero_vel_dist{}; std::optional determined_desired_margin{}; - const double limit_margin_for_unknown = 3.0; - const double preffered_acc_for_unknown = -3.0; - const auto closest_stop_obstacles = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); for (const auto & stop_obstacle : closest_stop_obstacles) { @@ -308,9 +305,9 @@ std::vector PlannerInterface::generateStopTrajectory( motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.ego_pose.position) + calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, preffered_acc_for_unknown); + planner_data.ego_vel, longitudinal_info_.limit_max_accel, preffered_acc_for_unknown_); const double liimit_margin_stop_dist = - std::max(0.0, dist_to_collide_on_ref_traj - limit_margin_for_unknown); + 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, liimit_margin_stop_dist); } From b356f6e4e1853aa4aaea08e792d8e35240b32370 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 29 Oct 2024 19:35:21 +0900 Subject: [PATCH 4/8] add universe params Signed-off-by: Yuki Takagi --- .../config/obstacle_cruise_planner.param.yaml | 2 ++ 1 file changed, 2 insertions(+) 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..fd07d643489a5 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: 3.0 + preffered_acc_for_unknown: -3.0 From 6a3ff6c0ce883113c38382838493472ae80458c2 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 29 Oct 2024 19:39:03 +0900 Subject: [PATCH 5/8] delete null line Signed-off-by: Yuki Takagi --- planning/obstacle_cruise_planner/src/node.cpp | 1 - planning/obstacle_cruise_planner/src/planner_interface.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 6deaaaa2e26c0..44a021412cfed 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -452,7 +452,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_, preffered_acc_for_unknown_); - } { // stop/cruise/slow down obstacle type diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 909e661ff7c0d..5ef6d3581c182 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -323,7 +323,6 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.ego_vel, longitudinal_info_.limit_max_accel, stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); - if (candidate_zero_vel_dist > distance_to_judge_suddenness) { return longitudinal_info_.limit_min_accel; } From 1faa0b5189b55727a6191bab035a0b87d7a5351c Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 29 Oct 2024 19:54:03 +0900 Subject: [PATCH 6/8] fix spell Signed-off-by: Yuki Takagi --- .../config/obstacle_cruise_planner.param.yaml | 2 +- .../include/obstacle_cruise_planner/node.hpp | 2 +- .../include/obstacle_cruise_planner/planner_interface.hpp | 6 +++--- planning/obstacle_cruise_planner/src/node.cpp | 8 ++++---- .../obstacle_cruise_planner/src/planner_interface.cpp | 2 +- 5 files changed, 10 insertions(+), 10 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 fd07d643489a5..5f47642c7c488 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -223,4 +223,4 @@ 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: 3.0 - preffered_acc_for_unknown: -3.0 + preferred_acc_for_unknown: -3.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 42d0b73a2d8c9..a879e90b6be87 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -111,7 +111,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; double limit_margin_for_unknown_; - double preffered_acc_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 06f74b1f94b7b..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 @@ -66,7 +66,7 @@ class PlannerInterface 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 limit_margin_for_unknown, const double preffered_acc_for_unknown) + 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; @@ -76,7 +76,7 @@ class PlannerInterface 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; - preffered_acc_for_unknown_ = preffered_acc_for_unknown; + preferred_acc_for_unknown_ = preferred_acc_for_unknown; } std::vector generateStopTrajectory( @@ -127,7 +127,7 @@ class PlannerInterface double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; double limit_margin_for_unknown_; - double preffered_acc_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 44a021412cfed..c1f47bdb7b656 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -446,12 +446,12 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); limit_margin_for_unknown_ = declare_parameter("stop.limit_margin_for_unknown"); - preffered_acc_for_unknown_ = declare_parameter("stop.preffered_acc_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_, limit_margin_for_unknown_, - preffered_acc_for_unknown_); + preferred_acc_for_unknown_); } { // stop/cruise/slow down obstacle type @@ -502,13 +502,13 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( tier4_autoware_utils::updateParam( parameters, "stop.limit_margin_for_unknown", limit_margin_for_unknown_); tier4_autoware_utils::updateParam( - parameters, "stop.preffered_acc_for_unknown", preffered_acc_for_unknown_); + 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_, limit_margin_for_unknown_, - preffered_acc_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 5ef6d3581c182..caa8c29ac937a 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -305,7 +305,7 @@ std::vector PlannerInterface::generateStopTrajectory( motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.ego_pose.position) + calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, preffered_acc_for_unknown_); + planner_data.ego_vel, longitudinal_info_.limit_max_accel, preferred_acc_for_unknown_); const double liimit_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) { From c467da61af716acae8e6870f2ded5e17dbcb5610 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 30 Oct 2024 11:08:29 +0900 Subject: [PATCH 7/8] fix Signed-off-by: Yuki Takagi --- planning/obstacle_cruise_planner/src/planner_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index caa8c29ac937a..1d1a39d0af282 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -306,10 +306,10 @@ std::vector PlannerInterface::generateStopTrajectory( 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 liimit_margin_stop_dist = + 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, liimit_margin_stop_dist); + candidate_zero_vel_dist = std::min(pref_acc_stop_dist, limit_margin_stop_dist); } } From 24bddf4d298f0f1f7314d9dcdfbb434dd5094199 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 30 Oct 2024 17:29:00 +0900 Subject: [PATCH 8/8] change universe params Signed-off-by: Yuki Takagi --- .../config/obstacle_cruise_planner.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 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 5f47642c7c488..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,5 +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: 3.0 - preferred_acc_for_unknown: -3.0 + limit_margin_for_unknown: 6.0 + preferred_acc_for_unknown: -6.0