feat(autoware_trajectory_optimizer): add external velocity limit subscription (#12023)

* add external velocity limit subscription

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* change topic name in code

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* subscribe to the external velocity limiter selector topic and not directly the API's

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* make the default velocity limit the same as the common_param velocity

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* Add /output/current_velocity_limit_mps publisher

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* output right topic name

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* change velocity limit input to API only, add common param to sync default velocity limit to that of Autoware common.param.yaml

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* change launch to use var name for the input external velocity for consistency

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* add mps to var name for consistency

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

* update dependencies in package.xml

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/autoware_trajectory_optimizer/config/plugins/trajectory_velocity_optimizer.param.yaml

@@ -10,8 +10,7 @@
       target_pull_out_acc_mps2: 1.0   # [m/s²] Target acceleration during pull-out
       set_engage_speed: false         # Set minimum speed during engage
 
-      # Speed limiting
-      max_speed_mps: 13.88             # [m/s] Maximum allowed speed (30 km/h default)
+      # Speed limiting (uses external_velocity_limit topic to set max speed)
       limit_speed: true               # Enable speed limiting
 
       # Lateral acceleration limiting

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_velocity_optimizer.hpp

@@ -20,12 +20,15 @@
 #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
 
 #include <autoware_utils/system/time_keeper.hpp>
+#include <autoware_utils_rclcpp/polling_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_planning_msgs/msg/velocity_limit.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -34,15 +37,16 @@ namespace autoware::trajectory_optimizer::plugin
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using autoware::velocity_smoother::JerkFilteredSmoother;
+using autoware_internal_planning_msgs::msg::VelocityLimit;
 
 struct TrajectoryVelocityOptimizerParams
 {
   double nearest_dist_threshold_m{1.5};
   double nearest_yaw_threshold_deg{60.0};
   double target_pull_out_speed_mps{1.0};
   double target_pull_out_acc_mps2{1.0};
-  double max_speed_mps{8.33};
   double max_lateral_accel_mps2{1.5};
+  double default_max_velocity_mps{8.33};  // 30 km/h
   bool set_engage_speed{false};
   bool limit_speed{true};
   bool limit_lateral_acceleration{false};
@@ -55,6 +59,9 @@ class TrajectoryVelocityOptimizer : public TrajectoryOptimizerPluginBase
   TrajectoryVelocityOptimizer() = default;
   ~TrajectoryVelocityOptimizer() = default;
 
+  void initialize(
+    const std::string & name, rclcpp::Node * node_ptr,
+    const std::shared_ptr<autoware_utils_debug::TimeKeeper> & time_keeper) override;
   void set_up_velocity_smoother(
     rclcpp::Node * node_ptr, const std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper);
   void optimize_trajectory(
@@ -67,6 +74,9 @@ class TrajectoryVelocityOptimizer : public TrajectoryOptimizerPluginBase
 private:
   std::shared_ptr<JerkFilteredSmoother> jerk_filtered_smoother_{nullptr};
   TrajectoryVelocityOptimizerParams velocity_params_;
+  std::shared_ptr<autoware_utils_rclcpp::InterProcessPollingSubscriber<VelocityLimit>>
+    sub_planning_velocity_;
+  rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
 };
 }  // namespace autoware::trajectory_optimizer::plugin
 

planning/autoware_trajectory_optimizer/launch/trajectory_optimizer.launch.xml

@@ -1,12 +1,11 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="trajectory_optimizer_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/trajectory_optimizer.param.yaml"/>
-
   <arg name="jerk_filtered_smoother_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/velocity_smoothing/JerkFiltered.param.yaml"/>
   <arg name="velocity_smoother_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/velocity_smoothing/default_velocity_smoother.param.yaml"/>
   <arg name="common_smoother_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/velocity_smoothing/default_common.param.yaml"/>
-
   <arg name="elastic_band_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/trajectory_smoothing/elastic_band_smoother.param.yaml"/>
+  <arg name="common_param_path" default="$(find-pkg-share autoware_core_planning)/config/common.param.yaml"/>
 
   <arg name="trajectory_point_fixer_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/plugins/trajectory_point_fixer.param.yaml"/>
   <arg name="trajectory_extender_param_path" default="$(find-pkg-share autoware_trajectory_optimizer)/config/plugins/trajectory_extender.param.yaml"/>
@@ -18,11 +17,14 @@
 
   <arg name="input_traj" default="/planning/diffusion_planner/trajectory"/>
   <arg name="input_trajectories" default="/planning/diffusion_planner/candidate_trajectories"/>
+  <arg name="input_external_velocity_limit_mps" default="/planning/scenario_planning/max_velocity_default"/>
   <arg name="output_traj" default="/planning/trajectory"/>
   <arg name="output_trajectories" default="/planning/generator/candidate_trajectories"/>
+  <arg name="output_current_velocity_limit_mps" default="/planning/scenario_planning/current_max_velocity"/>
 
   <node pkg="autoware_trajectory_optimizer" exec="autoware_trajectory_optimizer_node" name="trajectory_optimizer_node" output="screen">
     <param from="$(var trajectory_optimizer_param_path)" allow_substs="true"/>
+    <param from="$(var common_param_path)"/>
     <param from="$(var jerk_filtered_smoother_param_path)" allow_substs="true"/>
     <param from="$(var velocity_smoother_param_path)" allow_substs="true"/>
     <param from="$(var common_smoother_param_path)" allow_substs="true"/>
@@ -36,8 +38,10 @@
     <param from="$(var trajectory_mpt_optimizer_param_path)" allow_substs="true"/>
     <remap from="~/output/trajectory" to="$(var output_traj)"/>
     <remap from="~/output/trajectories" to="$(var output_trajectories)"/>
+    <remap from="~/output/current_velocity_limit_mps" to="$(var output_current_velocity_limit_mps)"/>
     <remap from="~/input/traj" to="$(var input_traj)"/>
     <remap from="~/input/trajectories" to="$(var input_trajectories)"/>
+    <remap from="~/input/external_velocity_limit_mps" to="$(var input_external_velocity_limit_mps)"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
     <remap from="~/input/acceleration" to="/localization/acceleration"/>
   </node>

planning/autoware_trajectory_optimizer/package.xml

@@ -35,6 +35,8 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <exec_depend>autoware_core_planning</exec_depend>
+
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

planning/autoware_trajectory_optimizer/schema/trajectory_optimizer.schema.json

@@ -394,12 +394,6 @@
               "minimum": 0,
               "description": "Target acceleration during pull-out maneuver [m/s²]"
             },
-            "max_speed_mps": {
-              "type": "number",
-              "default": 8.33,
-              "minimum": 0,
-              "description": "Maximum allowed speed [m/s]"
-            },
             "max_lateral_accel_mps2": {
               "type": "number",
               "default": 1.5,
@@ -432,7 +426,6 @@
             "nearest_yaw_threshold_deg",
             "target_pull_out_speed_mps",
             "target_pull_out_acc_mps2",
-            "max_speed_mps",
             "max_lateral_accel_mps2",
             "set_engage_speed",
             "limit_speed",

planning/autoware_trajectory_optimizer/src/trajectory_optimizer_plugins/trajectory_velocity_optimizer.cpp

@@ -19,14 +19,36 @@
 #include <autoware_utils_math/unit_conversion.hpp>
 #include <autoware_utils_rclcpp/parameter.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include <rclcpp/logging.hpp>
 
 #include <memory>
 #include <string>
 #include <vector>
-
 namespace autoware::trajectory_optimizer::plugin
 {
 
+void TrajectoryVelocityOptimizer::initialize(
+  const std::string & name, rclcpp::Node * node_ptr,
+  const std::shared_ptr<autoware_utils_debug::TimeKeeper> & time_keeper)
+{
+  TrajectoryOptimizerPluginBase::initialize(name, node_ptr, time_keeper);
+
+  set_up_params();
+
+  sub_planning_velocity_ =
+    std::make_shared<autoware_utils_rclcpp::InterProcessPollingSubscriber<VelocityLimit>>(
+      node_ptr, "~/input/external_velocity_limit_mps", rclcpp::QoS{1});
+
+  pub_velocity_limit_ = node_ptr->create_publisher<VelocityLimit>(
+    "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
+
+  // publish default max velocity
+  VelocityLimit max_vel_msg{};
+  max_vel_msg.stamp = node_ptr->now();
+  max_vel_msg.max_velocity = static_cast<float>(velocity_params_.default_max_velocity_mps);
+  pub_velocity_limit_->publish(max_vel_msg);
+}
+
 void TrajectoryVelocityOptimizer::set_up_velocity_smoother(
   rclcpp::Node * node_ptr, const std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper)
 {
@@ -51,7 +73,6 @@ void TrajectoryVelocityOptimizer::optimize_trajectory(
   const auto & current_linear_acceleration = current_acceleration.accel.accel.linear.x;
   const double & target_pull_out_speed_mps = velocity_params_.target_pull_out_speed_mps;
   const double & target_pull_out_acc_mps2 = velocity_params_.target_pull_out_acc_mps2;
-  const double & max_speed_mps = velocity_params_.max_speed_mps;
 
   if (velocity_params_.limit_lateral_acceleration) {
     trajectory_velocity_optimizer_utils::limit_lateral_acceleration(
@@ -71,8 +92,14 @@ void TrajectoryVelocityOptimizer::optimize_trajectory(
   }
 
   if (velocity_params_.limit_speed) {
-    trajectory_velocity_optimizer_utils::set_max_velocity(
-      traj_points, static_cast<float>(max_speed_mps));
+    const auto external_velocity_limit = sub_planning_velocity_->take_data();
+    const auto max_speed_mps = (external_velocity_limit)
+                                 ? static_cast<float>(external_velocity_limit->max_velocity)
+                                 : static_cast<float>(velocity_params_.default_max_velocity_mps);
+    trajectory_velocity_optimizer_utils::set_max_velocity(traj_points, max_speed_mps);
+    if (external_velocity_limit) {
+      pub_velocity_limit_->publish(*external_velocity_limit);
+    }
   }
 
   if (velocity_params_.smooth_velocities) {
@@ -100,10 +127,10 @@ void TrajectoryVelocityOptimizer::set_up_params()
     *node_ptr, "trajectory_velocity_optimizer.target_pull_out_speed_mps");
   velocity_params_.target_pull_out_acc_mps2 = get_or_declare_parameter<double>(
     *node_ptr, "trajectory_velocity_optimizer.target_pull_out_acc_mps2");
-  velocity_params_.max_speed_mps =
-    get_or_declare_parameter<double>(*node_ptr, "trajectory_velocity_optimizer.max_speed_mps");
   velocity_params_.max_lateral_accel_mps2 = get_or_declare_parameter<double>(
     *node_ptr, "trajectory_velocity_optimizer.max_lateral_accel_mps2");
+  velocity_params_.default_max_velocity_mps =
+    get_or_declare_parameter<double>(*node_ptr, "max_vel");
   velocity_params_.set_engage_speed =
     get_or_declare_parameter<bool>(*node_ptr, "trajectory_velocity_optimizer.set_engage_speed");
   velocity_params_.limit_speed =
@@ -131,8 +158,6 @@ rcl_interfaces::msg::SetParametersResult TrajectoryVelocityOptimizer::on_paramet
   update_param(
     parameters, "trajectory_velocity_optimizer.target_pull_out_acc_mps2",
     velocity_params_.target_pull_out_acc_mps2);
-  update_param(
-    parameters, "trajectory_velocity_optimizer.max_speed_mps", velocity_params_.max_speed_mps);
   update_param(
     parameters, "trajectory_velocity_optimizer.max_lateral_accel_mps2",
     velocity_params_.max_lateral_accel_mps2);