Skip to content

Commit

Permalink
Revert "feat(trajectory_follower): publsih control horzion (autowaref…
Browse files Browse the repository at this point in the history 
…oundation#8977)"

This reverts commit 1cb9dfa.
  • Loading branch information
@kosuke55
kosuke55 committed Nov 27, 2024
1 parent e10614c commit 9487d16
Show file tree
Hide file tree
Showing 12 changed files with 22 additions and 298 deletions.
5 changes: 1 addition & 4 deletions control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
#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 @@ -39,7 +38,6 @@
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 @@ -452,8 +450,7 @@ class MPC
*/
bool calculateMPC(
const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
LateralHorizon & ctrl_cmd_horizon);
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic);

/**
* @brief Set the reference trajectory to be followed.
Expand Down
9 changes: 0 additions & 9 deletions ...mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
#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 @@ -50,7 +49,6 @@ 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 @@ -216,13 +214,6 @@ 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: 1 addition & 15 deletions control/autoware_mpc_lateral_controller/src/mpc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,7 @@ MPC::MPC(rclcpp::Node & node)

bool MPC::calculateMPC(
const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
LateralHorizon & ctrl_cmd_horizon)
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic)
{
// 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 @@ -125,19 +124,6 @@ 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
25 changes: 4 additions & 21 deletions control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -276,10 +276,8 @@ 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,
ctrl_cmd_horizon);
m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);

m_is_mpc_solved = is_mpc_solved; // for diagnostic updater

Expand All @@ -306,13 +304,9 @@ 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 & cmd_horizon) -> trajectory_follower::LateralOutput {
const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
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 @@ -331,7 +325,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, ctrl_cmd_horizon);
return createLateralOutput(m_ctrl_cmd_prev, false);
}

if (!is_mpc_solved) {
Expand All @@ -340,7 +334,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
}

m_ctrl_cmd_prev = ctrl_cmd;
return createLateralOutput(ctrl_cmd, is_mpc_solved, ctrl_cmd_horizon);
return createLateralOutput(ctrl_cmd, is_mpc_solved);
}

bool MpcLateralController::isSteerConverged(const Lateral & cmd) const
Expand Down Expand Up @@ -471,17 +465,6 @@ 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: 13 additions & 59 deletions control/autoware_mpc_lateral_controller/test/test_mpc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@
#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 @@ -43,7 +41,6 @@
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 @@ -212,14 +209,10 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -248,14 +241,10 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -279,14 +268,10 @@ 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, ctrl_cmd_horizon));
EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -311,14 +296,10 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -345,14 +326,10 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -380,14 +357,10 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -409,14 +382,10 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -437,37 +406,24 @@ 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, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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);
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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);
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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);
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
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 @@ -490,13 +446,11 @@ 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, ctrl_cmd_horizon));
EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));

// 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,
ctrl_cmd_horizon));
neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag));
}
} // namespace autoware::motion::control::mpc_lateral_controller
8 changes: 1 addition & 7 deletions control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -433,8 +433,6 @@ 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 @@ -444,15 +442,11 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
// calculate control command
const Motion ctrl_cmd = calcCtrlCmd(control_data);

// create control command
// publish 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 9487d16

Please sign in to comment.