feat(velocity_smoother, external_velocity_limit_selector): enable str…

…onger acceleration when requested (#9502) * change max acceleration and max jerk according to external velocity request Signed-off-by: Go Sakayori <[email protected]> * modify external velocity limit selector Signed-off-by: Go Sakayori <[email protected]> * fix external velocity limit selector Signed-off-by: Go Sakayori <[email protected]> * fix format Signed-off-by: Go Sakayori <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]> Signed-off-by: Go Sakayori <[email protected]>
autowarefoundation · Dec 2, 2024 · 889b649 · 889b649
1 parent 4479e94
commit 889b649
Show file tree

Hide file tree

Showing 6 changed files with 81 additions and 7 deletions.
diff --git a/...clude/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/...clude/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp
@@ -22,7 +22,6 @@
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 
-#include <deque>
 #include <memory>
 #include <string>
 #include <unordered_map>

diff --git a/...g/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/...g/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 <deque>
+#include <cstddef>
 #include <memory>
+#include <ostream>
 #include <string>
 #include <unordered_map>
 #include <utility>
-#include <vector>
 
 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
@@ -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/.../autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/.../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
@@ -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<TrajectoryPoints> debug_trajectories;
   if (!smoother_->apply(
         initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories,
@@ -791,12 +818,15 @@ std::pair<Motion, VelocitySmootherNode::InitializeType> 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
@@ -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;