From 4728922fcf74f278a972697a7c01caf6ab6843ec Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 27 Sep 2024 17:19:44 +0900 Subject: [PATCH] feat(trajectory_follower): publsih control horzion Signed-off-by: kosuke55 --- .../autoware/mpc_lateral_controller/mpc.hpp | 5 +- .../mpc_lateral_controller.hpp | 9 ++ .../src/mpc.cpp | 16 ++- .../src/mpc_lateral_controller.cpp | 25 ++++- .../test/test_mpc.cpp | 39 ++++--- .../src/pid_longitudinal_controller.cpp | 8 +- .../control_horizon.hpp | 42 ++++++++ .../lateral_controller_base.hpp | 5 +- .../longitudinal_controller_base.hpp | 5 +- .../controller_node.hpp | 10 ++ .../src/controller_node.cpp | 101 ++++++++++++++++++ 11 files changed, 243 insertions(+), 22 deletions(-) create mode 100644 control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/control_horizon.hpp 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 index 36a79cc95728e..0e4691822326f 100644 --- 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/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index 09399d1fa2dce..ba0a17922ec0c 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_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 #include #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_horizion Control command horizon to be created. + * @return Created control command horizon. + */ + LateralHorizon createCtrlCmdHorizonMsg(const LateralHorizon & ctrl_cmd_horizion) 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 index 0f350dc40ad0e..f77e686437cea 100644 --- 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.push_back(ctrl_cmd); + for (auto it = std::next(Uex.begin()); it != Uex.end(); ++it) { + Lateral lateral{}; + lateral.steering_tire_angle = static_cast(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 index f4ba74d74f246..3648e8a79246a 100644 --- 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_horion) -> trajectory_follower::LateralOutput { trajectory_follower::LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); + output.control_cmd_horizon = createCtrlCmdHorizonMsg(cmd_horion); // 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_horizion) const +{ + auto out = ctrl_cmd_horizion; + 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 index c4a67552a6083..21ab344fc80f4 100644 --- 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 + #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,8 +212,9 @@ 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); } @@ -241,8 +245,9 @@ 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); } @@ -268,8 +273,9 @@ 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); } @@ -296,8 +302,9 @@ 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); } @@ -326,8 +333,9 @@ 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); } @@ -357,8 +365,9 @@ 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); } @@ -382,8 +391,9 @@ 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); } @@ -406,21 +416,22 @@ 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)); + 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)); + 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)); + 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)); @@ -446,11 +457,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 index e3cdc4505c037..9bc9a5cd59ac3 100644 --- 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); diff --git a/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/control_horizon.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/control_horizon.hpp new file mode 100644 index 0000000000000..980568cc73dc6 --- /dev/null +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/control_horizon.hpp @@ -0,0 +1,42 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__CONTROL_HORIZON_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__CONTROL_HORIZON_HPP_ + +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" + +#include + +namespace autoware::motion::control::trajectory_follower +{ + +using autoware_control_msgs::msg::Lateral; +using autoware_control_msgs::msg::Longitudinal; + +struct LateralHorizon +{ + double time_step_ms; + std::vector controls; +}; + +struct LongitudinalHorizon +{ + double time_step_ms; + std::vector controls; +}; + +} // namespace autoware::motion::control::trajectory_follower +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__CONTROL_HORIZON_HPP_ diff --git a/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp index 891b3982ddf49..4a3ddee78f6ad 100644 --- a/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ #define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#include "autoware/trajectory_follower_base/control_horizon.hpp" #include "autoware/trajectory_follower_base/input_data.hpp" #include "autoware/trajectory_follower_base/sync_data.hpp" #include "rclcpp/rclcpp.hpp" @@ -24,9 +25,11 @@ #include namespace autoware::motion::control::trajectory_follower { +using autoware_control_msgs::msg::Lateral; struct LateralOutput { - autoware_control_msgs::msg::Lateral control_cmd; + Lateral control_cmd; + LateralHorizon control_cmd_horizon; LateralSyncData sync_data; }; diff --git a/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp index 176141572d6a8..3c440c5a6dfb6 100644 --- a/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ #define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#include "autoware/trajectory_follower_base/control_horizon.hpp" #include "autoware/trajectory_follower_base/input_data.hpp" #include "autoware/trajectory_follower_base/sync_data.hpp" #include "rclcpp/rclcpp.hpp" @@ -25,9 +26,11 @@ namespace autoware::motion::control::trajectory_follower { +using autoware_control_msgs::msg::Longitudinal; struct LongitudinalOutput { - autoware_control_msgs::msg::Longitudinal control_cmd; + Longitudinal control_cmd; + LongitudinalHorizon control_cmd_horizon; LongitudinalSyncData sync_data; }; class LongitudinalControllerBase diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index a5f8665328f34..3c9063840daa3 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ #define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#include "autoware/trajectory_follower_base/control_horizon.hpp" #include "autoware/trajectory_follower_base/lateral_controller_base.hpp" #include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" #include "autoware/trajectory_follower_node/visibility_control.hpp" @@ -33,6 +34,7 @@ #include #include "autoware_control_msgs/msg/control.hpp" +#include "autoware_control_msgs/msg/control_horizon.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" @@ -41,6 +43,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include #include #include @@ -50,13 +53,16 @@ namespace autoware::motion::control { +using trajectory_follower::LateralHorizon; using trajectory_follower::LateralOutput; +using trajectory_follower::LongitudinalHorizon; using trajectory_follower::LongitudinalOutput; namespace trajectory_follower_node { using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_control_msgs::msg::ControlHorizon; using tier4_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -104,6 +110,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; rclcpp::Publisher::SharedPtr debug_marker_pub_; + rclcpp::Publisher::SharedPtr control_cmd_horizon_pub_; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr current_trajectory_ptr_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; @@ -134,6 +141,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void publishDebugMarker( const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; + static std::optional createControlHorizon( + const LateralHorizon & lateral_horizon, const LongitudinalHorizon & longitudinal_horizon, + const rclcpp::Time & stamp); std::unique_ptr logger_configure_; diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index d5167f5096786..7043754e20c0d 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -19,6 +19,8 @@ #include "autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" +#include + #include #include #include @@ -26,6 +28,24 @@ #include #include +namespace +{ +template +std::vector resizeHorizonByZeroOrderHold( + const std::vector & original_horizon, const double original_time_step_ms, + const double new_time_step_ms) +{ + std::vector resized_horizon; + const int step_factor = static_cast(original_time_step_ms / new_time_step_ms); + for (const auto & command : original_horizon) { + for (int i = 0; i < step_factor; ++i) { + resized_horizon.push_back(command); + } + } + return resized_horizon; +} +} // namespace + namespace autoware::motion::control::trajectory_follower_node { Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) @@ -74,6 +94,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control debug_marker_pub_ = create_publisher("~/output/debug_marker", rclcpp::QoS{1}); + control_cmd_horizon_pub_ = create_publisher( + "~/experimental/control_cmd_horizon", 1); + // Timer { const auto period_ns = std::chrono::duration_cast( @@ -215,6 +238,15 @@ void Controller::callbackTimerControl() // 6. publish debug published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp); publishDebugMarker(*input_data, lat_out); + + // 7. publish experimental topic + // NOTE: It is possible that using control_horizon could be expected to enhance performance, + // but it is not a formal interface topic, only an experimental one. + const auto control_horizon = + createControlHorizon(lat_out.control_cmd_horizon, lon_out.control_cmd_horizon, this->now()); + if (control_horizon.has_value()) { + control_cmd_horizon_pub_->publish(control_horizon.value()); + } } void Controller::publishDebugMarker( @@ -254,6 +286,75 @@ void Controller::publishProcessingTime( pub->publish(msg); } +std::optional Controller::createControlHorizon( + const LateralHorizon & lateral_horizon, const LongitudinalHorizon & longitudinal_horizon, + const rclcpp::Time & stamp) +{ + if (lateral_horizon.controls.empty() || longitudinal_horizon.controls.empty()) { + return std::nullopt; + } + + autoware_control_msgs::msg::ControlHorizon control_horizon{}; + control_horizon.stamp = stamp; + + // If either of the horizons has only one control, repeat the control to match the other horizon. + if (lateral_horizon.controls.size() == 1) { + control_horizon.time_step_ms = longitudinal_horizon.time_step_ms; + const auto lateral = lateral_horizon.controls.front(); + for (const auto & longitudinal : longitudinal_horizon.controls) { + autoware_control_msgs::msg::Control control; + control.longitudinal = longitudinal; + control.lateral = lateral; + control.stamp = stamp; + control_horizon.controls.push_back(control); + } + return control_horizon; + } + if (longitudinal_horizon.controls.size() == 1) { + control_horizon.time_step_ms = lateral_horizon.time_step_ms; + const auto longitudinal = longitudinal_horizon.controls.front(); + for (const auto & lateral : lateral_horizon.controls) { + autoware_control_msgs::msg::Control control; + control.longitudinal = longitudinal; + control.lateral = lateral; + control.stamp = stamp; + control_horizon.controls.push_back(control); + } + return control_horizon; + } + + // If both horizons have multiple controls, align the time steps and zero-order hold the controls. + // calculate greatest common divisor of time steps + const auto gcd_double = [](const double a, const double b) { + const double precision = 1e9; + const int int_a = static_cast(round(a * precision)); + const int int_b = static_cast(round(b * precision)); + return static_cast(std::gcd(int_a, int_b)) / precision; + }; + const double time_step_ms = + gcd_double(lateral_horizon.time_step_ms, longitudinal_horizon.time_step_ms); + control_horizon.time_step_ms = time_step_ms; + + const auto lateral_controls = resizeHorizonByZeroOrderHold( + lateral_horizon.controls, lateral_horizon.time_step_ms, time_step_ms); + const auto longitudinal_controls = resizeHorizonByZeroOrderHold( + longitudinal_horizon.controls, longitudinal_horizon.time_step_ms, time_step_ms); + + if (lateral_controls.size() != longitudinal_controls.size()) { + return std::nullopt; + } + + const size_t num_steps = lateral_controls.size(); + for (size_t i = 0; i < num_steps; ++i) { + autoware_control_msgs::msg::Control control{}; + control.stamp = stamp; + control.lateral = lateral_controls.at(i); + control.longitudinal = longitudinal_controls.at(i); + control_horizon.controls.push_back(control); + } + return control_horizon; +} + } // namespace autoware::motion::control::trajectory_follower_node #include "rclcpp_components/register_node_macro.hpp"