Skip to content

Commit

Permalink
feat(trajectory_follower): publsih control horzion (autowarefoundatio…
Browse files Browse the repository at this point in the history
…n#8977)

* feat(trajectory_follower): publsih control horzion

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typo

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* rename functions and minor refactor

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add option to enable horizon pub

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add tests for horizon

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* update docs

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* rename to ~/debug/control_cmd_horizon

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
kosuke55 committed Nov 26, 2024
1 parent d7c2631 commit 1cb9dfa
Show file tree
Hide file tree
Showing 12 changed files with 298 additions and 22 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand Down Expand Up @@ -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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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.
Expand Down
16 changes: 15 additions & 1 deletion control/autoware_mpc_lateral_controller/src/mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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.
Expand All @@ -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) {
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand Down
72 changes: 59 additions & 13 deletions control/autoware_mpc_lateral_controller/test/test_mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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);

Expand Down
Loading

0 comments on commit 1cb9dfa

Please sign in to comment.