feat(trajectory_follower): publsih control horzion (#8977)

* feat(trajectory_follower): publsih control horzion Signed-off-by: kosuke55 <[email protected]> * fix typo Signed-off-by: kosuke55 <[email protected]> * rename functions and minor refactor Signed-off-by: kosuke55 <[email protected]> * add option to enable horizon pub Signed-off-by: kosuke55 <[email protected]> * add tests for horizon Signed-off-by: kosuke55 <[email protected]> * update docs Signed-off-by: kosuke55 <[email protected]> * rename to ~/debug/control_cmd_horizon Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Nov 14, 2024 · b4603fd · b4603fd
1 parent c6c3b1c
commit b4603fd
Show file tree

Hide file tree

Showing 12 changed files with 298 additions and 22 deletions.
diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp
@@ -20,6 +20,7 @@
 #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
 #include "autoware/mpc_lateral_controller/steering_predictor.hpp"
 #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+#include "autoware/trajectory_follower_base/control_horizon.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_control_msgs/msg/lateral.hpp"
@@ -38,6 +39,7 @@
 namespace autoware::motion::control::mpc_lateral_controller
 {
 
+using autoware::motion::control::trajectory_follower::LateralHorizon;
 using autoware_control_msgs::msg::Lateral;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_vehicle_msgs::msg::SteeringReport;
@@ -450,7 +452,8 @@ class MPC
    */
   bool calculateMPC(
     const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
-    Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic);
+    Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
+    LateralHorizon & ctrl_cmd_horizon);
 
   /**
    * @brief Set the reference trajectory to be followed.

diff --git a/...mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp b/...mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp
@@ -22,6 +22,7 @@
 #include "autoware/trajectory_follower_base/lateral_controller_base.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include <autoware/trajectory_follower_base/control_horizon.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 
 #include "autoware_control_msgs/msg/lateral.hpp"
@@ -49,6 +50,7 @@ using autoware_vehicle_msgs::msg::SteeringReport;
 using nav_msgs::msg::Odometry;
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 using tier4_debug_msgs::msg::Float32Stamped;
+using trajectory_follower::LateralHorizon;
 
 class MpcLateralController : public trajectory_follower::LateralControllerBase
 {
@@ -214,6 +216,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
    */
   Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd);
 
+  /**
+   * @brief Create the control command horizon message.
+   * @param ctrl_cmd_horizon Control command horizon to be created.
+   * @return Created control command horizon.
+   */
+  LateralHorizon createCtrlCmdHorizonMsg(const LateralHorizon & ctrl_cmd_horizon) const;
+
   /**
    * @brief Publish the predicted future trajectory.
    * @param predicted_traj Predicted future trajectory to be published.

diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp
@@ -39,7 +39,8 @@ MPC::MPC(rclcpp::Node & node)
 
 bool MPC::calculateMPC(
   const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
-  Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic)
+  Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
+  LateralHorizon & ctrl_cmd_horizon)
 {
   // since the reference trajectory does not take into account the current velocity of the ego
   // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
@@ -124,6 +125,19 @@ bool MPC::calculateMPC(
   diagnostic =
     generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
 
+  // create LateralHorizon command
+  ctrl_cmd_horizon.time_step_ms = prediction_dt * 1000.0;
+  ctrl_cmd_horizon.controls.clear();
+  ctrl_cmd_horizon.controls.push_back(ctrl_cmd);
+  for (auto it = std::next(Uex.begin()); it != Uex.end(); ++it) {
+    Lateral lateral{};
+    lateral.steering_tire_angle = static_cast<float>(std::clamp(*it, -m_steer_lim, m_steer_lim));
+    lateral.steering_tire_rotation_rate =
+      (lateral.steering_tire_angle - ctrl_cmd_horizon.controls.back().steering_tire_angle) /
+      m_ctrl_period;
+    ctrl_cmd_horizon.controls.push_back(lateral);
+  }
+
   return true;
 }
 

diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp
@@ -276,8 +276,10 @@ trajectory_follower::LateralOutput MpcLateralController::run(
     m_is_ctrl_cmd_prev_initialized = true;
   }
 
+  trajectory_follower::LateralHorizon ctrl_cmd_horizon{};
   const bool is_mpc_solved = m_mpc->calculateMPC(
-    m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
+    m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values,
+    ctrl_cmd_horizon);
 
   m_is_mpc_solved = is_mpc_solved;  // for diagnostic updater
 
@@ -304,9 +306,13 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   publishPredictedTraj(predicted_traj);
   publishDebugValues(debug_values);
 
-  const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
+  const auto createLateralOutput =
+    [this](
+      const auto & cmd, const bool is_mpc_solved,
+      const auto & cmd_horizon) -> trajectory_follower::LateralOutput {
     trajectory_follower::LateralOutput output;
     output.control_cmd = createCtrlCmdMsg(cmd);
+    output.control_cmd_horizon = createCtrlCmdHorizonMsg(cmd_horizon);
     // To be sure current steering of the vehicle is desired steering angle, we need to check
     // following conditions.
     // 1. At the last loop, mpc should be solved because command should be optimized output.
@@ -325,7 +331,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
     }
     // Use previous command value as previous raw steer command
     m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
-    return createLateralOutput(m_ctrl_cmd_prev, false);
+    return createLateralOutput(m_ctrl_cmd_prev, false, ctrl_cmd_horizon);
   }
 
   if (!is_mpc_solved) {
@@ -334,7 +340,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   }
 
   m_ctrl_cmd_prev = ctrl_cmd;
-  return createLateralOutput(ctrl_cmd, is_mpc_solved);
+  return createLateralOutput(ctrl_cmd, is_mpc_solved, ctrl_cmd_horizon);
 }
 
 bool MpcLateralController::isSteerConverged(const Lateral & cmd) const
@@ -465,6 +471,17 @@ Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd)
   return out;
 }
 
+LateralHorizon MpcLateralController::createCtrlCmdHorizonMsg(
+  const LateralHorizon & ctrl_cmd_horizon) const
+{
+  auto out = ctrl_cmd_horizon;
+  const auto now = clock_->now();
+  for (auto & cmd : out.controls) {
+    cmd.stamp = now;
+  }
+  return out;
+}
+
 void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const
 {
   predicted_traj.header.stamp = clock_->now();

diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp
@@ -21,6 +21,8 @@
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 
+#include <autoware/trajectory_follower_base/control_horizon.hpp>
+
 #include "autoware_control_msgs/msg/lateral.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
@@ -41,6 +43,7 @@
 namespace autoware::motion::control::mpc_lateral_controller
 {
 
+using autoware::motion::control::trajectory_follower::LateralHorizon;
 using autoware_control_msgs::msg::Lateral;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -209,10 +212,14 @@ TEST_F(MPCTest, InitializeAndCalculate)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, InitializeAndCalculateRightTurn)
@@ -241,10 +248,14 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, OsqpCalculate)
@@ -268,10 +279,14 @@ TEST_F(MPCTest, OsqpCalculate)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, OsqpCalculateRightTurn)
@@ -296,10 +311,14 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, KinematicsNoDelayCalculate)
@@ -326,10 +345,14 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
@@ -357,10 +380,14 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, DynamicCalculate)
@@ -382,10 +409,14 @@ TEST_F(MPCTest, DynamicCalculate)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, MultiSolveWithBuffer)
@@ -406,24 +437,37 @@ TEST_F(MPCTest, MultiSolveWithBuffer)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_zero, default_velocity);
 
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
   EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
   EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
   EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
-  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
+  ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
   EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
+  EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
+  EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
 
 TEST_F(MPCTest, FailureCases)
@@ -446,11 +490,13 @@ TEST_F(MPCTest, FailureCases)
   Lateral ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
+  LateralHorizon ctrl_cmd_horizon;
   const auto odom = makeOdometry(pose_far, default_velocity);
-  EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
 
   // Calculate MPC with a fast velocity to make the prediction go further than the reference path
   EXPECT_FALSE(mpc->calculateMPC(
-    neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag));
+    neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag,
+    ctrl_cmd_horizon));
 }
 }  // namespace autoware::motion::control::mpc_lateral_controller
diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -433,6 +433,8 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
     publishDebugData(raw_ctrl_cmd, control_data);                       // publish debug data
     trajectory_follower::LongitudinalOutput output;
     output.control_cmd = cmd_msg;
+    output.control_cmd_horizon.controls.push_back(cmd_msg);
+    output.control_cmd_horizon.time_step_ms = 0.0;
     return output;
   }
 
@@ -442,11 +444,15 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
   // calculate control command
   const Motion ctrl_cmd = calcCtrlCmd(control_data);
 
-  // publish control command
+  // create control command
   const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel);
   trajectory_follower::LongitudinalOutput output;
   output.control_cmd = cmd_msg;
 
+  // create control command horizon
+  output.control_cmd_horizon.controls.push_back(cmd_msg);
+  output.control_cmd_horizon.time_step_ms = 0.0;
+
   // publish debug data
   publishDebugData(ctrl_cmd, control_data);