From 889b6494ab6d757286ed62e340ba5c40541dc4d4 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 2 Dec 2024 10:15:30 +0900 Subject: [PATCH] feat(velocity_smoother, external_velocity_limit_selector): enable stronger acceleration when requested (#9502) * change max acceleration and max jerk according to external velocity request Signed-off-by: Go Sakayori * modify external velocity limit selector Signed-off-by: Go Sakayori * fix external velocity limit selector Signed-off-by: Go Sakayori * fix format Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../external_velocity_limit_selector_node.hpp | 1 - .../external_velocity_limit_selector_node.cpp | 30 ++++++++++++++- .../autoware/velocity_smoother/node.hpp | 7 ++++ .../smoother/smoother_base.hpp | 2 + .../autoware_velocity_smoother/src/node.cpp | 38 +++++++++++++++++-- .../src/smoother/smoother_base.cpp | 10 +++++ 6 files changed, 81 insertions(+), 7 deletions(-) diff --git a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 2aa06079dadbc..6a8fae3bf394c 100644 --- a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -22,7 +22,6 @@ #include #include -#include #include #include #include diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 2052c8438bcfa..cf42763fc6b60 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -14,12 +14,12 @@ #include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" -#include +#include #include +#include #include #include #include -#include namespace autoware::external_velocity_limit_selector { @@ -34,12 +34,17 @@ VelocityLimit getHardestLimit( hardest_limit.max_velocity = node_param.max_vel; VelocityLimitConstraints normal_constraints{}; + normal_constraints.max_acceleration = node_param.normal.max_acc; normal_constraints.min_acceleration = node_param.normal.min_acc; normal_constraints.min_jerk = node_param.normal.min_jerk; normal_constraints.max_jerk = node_param.normal.max_jerk; double hardest_max_velocity = node_param.max_vel; double hardest_max_jerk = 0.0; + double hardest_max_acceleration = 0.0; + std::string hardest_max_acceleration_key; + size_t constraint_num = 0; + size_t acceleration_constraint_num = 0; for (const auto & limit : velocity_limits) { // guard nan, inf @@ -59,6 +64,19 @@ VelocityLimit getHardestLimit( ? limit.second.constraints : normal_constraints; + if (limit.second.use_constraints) { + constraint_num++; + if (limit.second.constraints.max_acceleration > normal_constraints.max_acceleration) { + acceleration_constraint_num++; + hardest_max_acceleration_key = limit.first; + } + } + + if (hardest_max_acceleration < limit.second.constraints.max_acceleration) { + hardest_max_acceleration_key = limit.first; + hardest_max_acceleration = limit.second.constraints.max_acceleration; + } + // find hardest jerk if (hardest_max_jerk < constraints.max_jerk) { hardest_limit.constraints = constraints; @@ -67,6 +85,14 @@ VelocityLimit getHardestLimit( } } + if (constraint_num > 0 && constraint_num == acceleration_constraint_num) { + if (velocity_limits.find(hardest_max_acceleration_key) != velocity_limits.end()) { + const auto constraints = velocity_limits.at(hardest_max_acceleration_key).constraints; + hardest_limit.constraints = constraints; + hardest_limit.use_constraints = true; + } + } + return hardest_limit; } diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 3f1c313e052da..069363d2f65e0 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -164,10 +164,17 @@ class VelocitySmootherNode : public rclcpp::Node bool plan_from_ego_speed_on_manual_mode = true; } node_param_{}; + struct AccelerationRequest + { + bool request{false}; + double max_acceleration{0.0}; + double max_jerk{0.0}; + }; struct ExternalVelocityLimit { double velocity{0.0}; // current external_velocity_limit double dist{0.0}; // distance to set external velocity limit + AccelerationRequest acceleration_request; std::string sender{""}; }; ExternalVelocityLimit diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index 7206a64ea32eb..d51431383f88a 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -83,6 +83,8 @@ class SmootherBase double getMinJerk() const; void setWheelBase(const double wheel_base); + void setMaxAccel(const double max_acceleration); + void setMaxJerk(const double max_jerk); void setParam(const BaseParam & param); BaseParam getBaseParam() const; diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index a358a1b61c11b..f47c84ab54b01 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -319,6 +319,7 @@ void VelocitySmootherNode::calcExternalVelocityLimit() autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!external_velocity_limit_ptr_) { + external_velocity_limit_.acceleration_request.request = false; return; } @@ -342,6 +343,20 @@ void VelocitySmootherNode::calcExternalVelocityLimit() external_velocity_limit_.dist = 0.0; } + const auto base_max_acceleration = get_parameter("normal.max_acc").as_double(); + const auto acceleration_request = + external_velocity_limit_ptr_->use_constraints && + base_max_acceleration < external_velocity_limit_ptr_->constraints.max_acceleration; + if ( + acceleration_request && + current_odometry_ptr_->twist.twist.linear.x < external_velocity_limit_ptr_->max_velocity) { + external_velocity_limit_.acceleration_request.request = true; + external_velocity_limit_.acceleration_request.max_acceleration = + external_velocity_limit_ptr_->constraints.max_acceleration; + external_velocity_limit_.acceleration_request.max_jerk = + external_velocity_limit_ptr_->constraints.max_jerk; + } + // calculate distance and maximum velocity // to decelerate to external velocity limit with jerk and acceleration // constraints. @@ -627,6 +642,18 @@ bool VelocitySmootherNode::smoothVelocity( clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + // Set maximum acceleration before applying smoother. Depends on acceleration request from + // external velocity limit + const double smoother_max_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : get_parameter("normal.max_acc").as_double(); + const double smoother_max_jerk = external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_jerk + : get_parameter("normal.max_jerk").as_double(); + smoother_->setMaxAccel(smoother_max_acceleration); + smoother_->setMaxJerk(smoother_max_jerk); + std::vector debug_trajectories; if (!smoother_->apply( initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, @@ -791,12 +818,15 @@ std::pair VelocitySmootherNode::ca "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist); - Motion initial_motion = {node_param_.engage_velocity, node_param_.engage_acceleration}; + const double engage_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : node_param_.engage_acceleration; + Motion initial_motion = {node_param_.engage_velocity, engage_acceleration}; return {initial_motion, InitializeType::ENGAGING}; - } else { - RCLCPP_DEBUG( - get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); } + RCLCPP_DEBUG( + get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); } else if (target_vel > 0.0) { RCLCPP_WARN_THROTTLE( get_logger(), *clock_, 3000, diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 46faf10fe4a62..5360da101ee8c 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -97,6 +97,16 @@ void SmootherBase::setWheelBase(const double wheel_base) base_param_.wheel_base = wheel_base; } +void SmootherBase::setMaxAccel(const double max_acceleration) +{ + base_param_.max_accel = max_acceleration; +} + +void SmootherBase::setMaxJerk(const double max_jerk) +{ + base_param_.max_jerk = max_jerk; +} + void SmootherBase::setParam(const BaseParam & param) { base_param_ = param;