Skip to content

Commit b4603fd

Browse files
kosuke55kosuke55
authored
feat(trajectory_follower): publsih control horzion (#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>
1 parent c6c3b1c commit b4603fd

File tree

12 files changed

+298
-22
lines changed

12 files changed

+298
-22
lines changed

‎control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
2121
#include "autoware/mpc_lateral_controller/steering_predictor.hpp"
2222
#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
23+
#include "autoware/trajectory_follower_base/control_horizon.hpp"
2324
#include "rclcpp/rclcpp.hpp"
2425

2526
#include "autoware_control_msgs/msg/lateral.hpp"
@@ -38,6 +39,7 @@
3839
namespace autoware::motion::control::mpc_lateral_controller
3940
{
4041

42+
using autoware::motion::control::trajectory_follower::LateralHorizon;
4143
using autoware_control_msgs::msg::Lateral;
4244
using autoware_planning_msgs::msg::Trajectory;
4345
using autoware_vehicle_msgs::msg::SteeringReport;
@@ -450,7 +452,8 @@ class MPC
450452
*/
451453
bool calculateMPC(
452454
const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
453-
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic);
455+
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
456+
LateralHorizon & ctrl_cmd_horizon);
454457

455458
/**
456459
* @brief Set the reference trajectory to be followed.

‎control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp

+9
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "autoware/trajectory_follower_base/lateral_controller_base.hpp"
2323
#include "rclcpp/rclcpp.hpp"
2424

25+
#include <autoware/trajectory_follower_base/control_horizon.hpp>
2526
#include <diagnostic_updater/diagnostic_updater.hpp>
2627

2728
#include "autoware_control_msgs/msg/lateral.hpp"
@@ -49,6 +50,7 @@ using autoware_vehicle_msgs::msg::SteeringReport;
4950
using nav_msgs::msg::Odometry;
5051
using tier4_debug_msgs::msg::Float32MultiArrayStamped;
5152
using tier4_debug_msgs::msg::Float32Stamped;
53+
using trajectory_follower::LateralHorizon;
5254

5355
class MpcLateralController : public trajectory_follower::LateralControllerBase
5456
{
@@ -214,6 +216,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
214216
*/
215217
Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd);
216218

219+
/**
220+
* @brief Create the control command horizon message.
221+
* @param ctrl_cmd_horizon Control command horizon to be created.
222+
* @return Created control command horizon.
223+
*/
224+
LateralHorizon createCtrlCmdHorizonMsg(const LateralHorizon & ctrl_cmd_horizon) const;
225+
217226
/**
218227
* @brief Publish the predicted future trajectory.
219228
* @param predicted_traj Predicted future trajectory to be published.

‎control/autoware_mpc_lateral_controller/src/mpc.cpp

+15-1
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@ MPC::MPC(rclcpp::Node & node)
3939

4040
bool MPC::calculateMPC(
4141
const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
42-
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic)
42+
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
43+
LateralHorizon & ctrl_cmd_horizon)
4344
{
4445
// since the reference trajectory does not take into account the current velocity of the ego
4546
// vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
@@ -124,6 +125,19 @@ bool MPC::calculateMPC(
124125
diagnostic =
125126
generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
126127

128+
// create LateralHorizon command
129+
ctrl_cmd_horizon.time_step_ms = prediction_dt * 1000.0;
130+
ctrl_cmd_horizon.controls.clear();
131+
ctrl_cmd_horizon.controls.push_back(ctrl_cmd);
132+
for (auto it = std::next(Uex.begin()); it != Uex.end(); ++it) {
133+
Lateral lateral{};
134+
lateral.steering_tire_angle = static_cast<float>(std::clamp(*it, -m_steer_lim, m_steer_lim));
135+
lateral.steering_tire_rotation_rate =
136+
(lateral.steering_tire_angle - ctrl_cmd_horizon.controls.back().steering_tire_angle) /
137+
m_ctrl_period;
138+
ctrl_cmd_horizon.controls.push_back(lateral);
139+
}
140+
127141
return true;
128142
}
129143

‎control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

+21-4
Original file line numberDiff line numberDiff line change
@@ -276,8 +276,10 @@ trajectory_follower::LateralOutput MpcLateralController::run(
276276
m_is_ctrl_cmd_prev_initialized = true;
277277
}
278278

279+
trajectory_follower::LateralHorizon ctrl_cmd_horizon{};
279280
const bool is_mpc_solved = m_mpc->calculateMPC(
280-
m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
281+
m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values,
282+
ctrl_cmd_horizon);
281283

282284
m_is_mpc_solved = is_mpc_solved; // for diagnostic updater
283285

@@ -304,9 +306,13 @@ trajectory_follower::LateralOutput MpcLateralController::run(
304306
publishPredictedTraj(predicted_traj);
305307
publishDebugValues(debug_values);
306308

307-
const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
309+
const auto createLateralOutput =
310+
[this](
311+
const auto & cmd, const bool is_mpc_solved,
312+
const auto & cmd_horizon) -> trajectory_follower::LateralOutput {
308313
trajectory_follower::LateralOutput output;
309314
output.control_cmd = createCtrlCmdMsg(cmd);
315+
output.control_cmd_horizon = createCtrlCmdHorizonMsg(cmd_horizon);
310316
// To be sure current steering of the vehicle is desired steering angle, we need to check
311317
// following conditions.
312318
// 1. At the last loop, mpc should be solved because command should be optimized output.
@@ -325,7 +331,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
325331
}
326332
// Use previous command value as previous raw steer command
327333
m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
328-
return createLateralOutput(m_ctrl_cmd_prev, false);
334+
return createLateralOutput(m_ctrl_cmd_prev, false, ctrl_cmd_horizon);
329335
}
330336

331337
if (!is_mpc_solved) {
@@ -334,7 +340,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
334340
}
335341

336342
m_ctrl_cmd_prev = ctrl_cmd;
337-
return createLateralOutput(ctrl_cmd, is_mpc_solved);
343+
return createLateralOutput(ctrl_cmd, is_mpc_solved, ctrl_cmd_horizon);
338344
}
339345

340346
bool MpcLateralController::isSteerConverged(const Lateral & cmd) const
@@ -465,6 +471,17 @@ Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd)
465471
return out;
466472
}
467473

474+
LateralHorizon MpcLateralController::createCtrlCmdHorizonMsg(
475+
const LateralHorizon & ctrl_cmd_horizon) const
476+
{
477+
auto out = ctrl_cmd_horizon;
478+
const auto now = clock_->now();
479+
for (auto & cmd : out.controls) {
480+
cmd.stamp = now;
481+
}
482+
return out;
483+
}
484+
468485
void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const
469486
{
470487
predicted_traj.header.stamp = clock_->now();

‎control/autoware_mpc_lateral_controller/test/test_mpc.cpp

+59-13
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121
#include "gtest/gtest.h"
2222
#include "rclcpp/rclcpp.hpp"
2323

24+
#include <autoware/trajectory_follower_base/control_horizon.hpp>
25+
2426
#include "autoware_control_msgs/msg/lateral.hpp"
2527
#include "autoware_planning_msgs/msg/trajectory.hpp"
2628
#include "autoware_planning_msgs/msg/trajectory_point.hpp"
@@ -41,6 +43,7 @@
4143
namespace autoware::motion::control::mpc_lateral_controller
4244
{
4345

46+
using autoware::motion::control::trajectory_follower::LateralHorizon;
4447
using autoware_control_msgs::msg::Lateral;
4548
using autoware_planning_msgs::msg::Trajectory;
4649
using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -209,10 +212,14 @@ TEST_F(MPCTest, InitializeAndCalculate)
209212
Lateral ctrl_cmd;
210213
Trajectory pred_traj;
211214
Float32MultiArrayStamped diag;
215+
LateralHorizon ctrl_cmd_horizon;
212216
const auto odom = makeOdometry(pose_zero, default_velocity);
213-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
217+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
214218
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
215219
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
220+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
221+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
222+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
216223
}
217224

218225
TEST_F(MPCTest, InitializeAndCalculateRightTurn)
@@ -241,10 +248,14 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)
241248
Lateral ctrl_cmd;
242249
Trajectory pred_traj;
243250
Float32MultiArrayStamped diag;
251+
LateralHorizon ctrl_cmd_horizon;
244252
const auto odom = makeOdometry(pose_zero, default_velocity);
245-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
253+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
246254
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
247255
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
256+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
257+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
258+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
248259
}
249260

250261
TEST_F(MPCTest, OsqpCalculate)
@@ -268,10 +279,14 @@ TEST_F(MPCTest, OsqpCalculate)
268279
Lateral ctrl_cmd;
269280
Trajectory pred_traj;
270281
Float32MultiArrayStamped diag;
282+
LateralHorizon ctrl_cmd_horizon;
271283
const auto odom = makeOdometry(pose_zero, default_velocity);
272-
EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
284+
EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
273285
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
274286
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
287+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
288+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
289+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
275290
}
276291

277292
TEST_F(MPCTest, OsqpCalculateRightTurn)
@@ -296,10 +311,14 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
296311
Lateral ctrl_cmd;
297312
Trajectory pred_traj;
298313
Float32MultiArrayStamped diag;
314+
LateralHorizon ctrl_cmd_horizon;
299315
const auto odom = makeOdometry(pose_zero, default_velocity);
300-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
316+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
301317
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
302318
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
319+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
320+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
321+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
303322
}
304323

305324
TEST_F(MPCTest, KinematicsNoDelayCalculate)
@@ -326,10 +345,14 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
326345
Lateral ctrl_cmd;
327346
Trajectory pred_traj;
328347
Float32MultiArrayStamped diag;
348+
LateralHorizon ctrl_cmd_horizon;
329349
const auto odom = makeOdometry(pose_zero, default_velocity);
330-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
350+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
331351
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
332352
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
353+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
354+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
355+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
333356
}
334357

335358
TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
@@ -357,10 +380,14 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
357380
Lateral ctrl_cmd;
358381
Trajectory pred_traj;
359382
Float32MultiArrayStamped diag;
383+
LateralHorizon ctrl_cmd_horizon;
360384
const auto odom = makeOdometry(pose_zero, default_velocity);
361-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
385+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
362386
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
363387
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
388+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
389+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
390+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
364391
}
365392

366393
TEST_F(MPCTest, DynamicCalculate)
@@ -382,10 +409,14 @@ TEST_F(MPCTest, DynamicCalculate)
382409
Lateral ctrl_cmd;
383410
Trajectory pred_traj;
384411
Float32MultiArrayStamped diag;
412+
LateralHorizon ctrl_cmd_horizon;
385413
const auto odom = makeOdometry(pose_zero, default_velocity);
386-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
414+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
387415
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
388416
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
417+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
418+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
419+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
389420
}
390421

391422
TEST_F(MPCTest, MultiSolveWithBuffer)
@@ -406,24 +437,37 @@ TEST_F(MPCTest, MultiSolveWithBuffer)
406437
Lateral ctrl_cmd;
407438
Trajectory pred_traj;
408439
Float32MultiArrayStamped diag;
440+
LateralHorizon ctrl_cmd_horizon;
409441
const auto odom = makeOdometry(pose_zero, default_velocity);
410442

411-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
443+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
412444
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
413445
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
414446
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
415-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
447+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
448+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
449+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
450+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
416451
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
417452
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
418453
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
419-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
454+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
455+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
456+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
457+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
420458
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
421459
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
422460
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
423-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
461+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
462+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
463+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
464+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
424465
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
425466
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
426467
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
468+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
469+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
470+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
427471
}
428472

429473
TEST_F(MPCTest, FailureCases)
@@ -446,11 +490,13 @@ TEST_F(MPCTest, FailureCases)
446490
Lateral ctrl_cmd;
447491
Trajectory pred_traj;
448492
Float32MultiArrayStamped diag;
493+
LateralHorizon ctrl_cmd_horizon;
449494
const auto odom = makeOdometry(pose_far, default_velocity);
450-
EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
495+
EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
451496

452497
// Calculate MPC with a fast velocity to make the prediction go further than the reference path
453498
EXPECT_FALSE(mpc->calculateMPC(
454-
neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag));
499+
neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag,
500+
ctrl_cmd_horizon));
455501
}
456502
} // namespace autoware::motion::control::mpc_lateral_controller

‎control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -433,6 +433,8 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
433433
publishDebugData(raw_ctrl_cmd, control_data); // publish debug data
434434
trajectory_follower::LongitudinalOutput output;
435435
output.control_cmd = cmd_msg;
436+
output.control_cmd_horizon.controls.push_back(cmd_msg);
437+
output.control_cmd_horizon.time_step_ms = 0.0;
436438
return output;
437439
}
438440

@@ -442,11 +444,15 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
442444
// calculate control command
443445
const Motion ctrl_cmd = calcCtrlCmd(control_data);
444446

445-
// publish control command
447+
// create control command
446448
const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel);
447449
trajectory_follower::LongitudinalOutput output;
448450
output.control_cmd = cmd_msg;
449451

452+
// create control command horizon
453+
output.control_cmd_horizon.controls.push_back(cmd_msg);
454+
output.control_cmd_horizon.time_step_ms = 0.0;
455+
450456
// publish debug data
451457
publishDebugData(ctrl_cmd, control_data);
452458

0 commit comments

Comments
 (0)