Skip to content

Commit 56100ef

Browse files
kosuke55kyoichi-sugahara
kosuke55
authored and
kyoichi-sugahara
committed
feat(trajectory_follower): publsih control horzion (autowarefoundation#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 afa7a52 commit 56100ef

File tree

12 files changed

+294
-22
lines changed

12 files changed

+294
-22
lines changed

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

+3-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 "autoware_mpc_lateral_controller/msg/mpc_debug.hpp"
2425
#include "rclcpp/rclcpp.hpp"
2526

@@ -40,6 +41,7 @@
4041
namespace autoware::motion::control::mpc_lateral_controller
4142
{
4243

44+
using autoware::motion::control::trajectory_follower::LateralHorizon;
4345
using autoware_control_msgs::msg::Lateral;
4446
using autoware_mpc_lateral_controller::msg::MpcDebug;
4547
using autoware_planning_msgs::msg::Trajectory;
@@ -491,7 +493,7 @@ class MPC
491493
bool calculateMPC(
492494
const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
493495
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
494-
const std::string & qp_solver_type = "osqp");
496+
LateralHorizon & ctrl_cmd_horizon, const std::string & qp_solver_type = "osqp");
495497

496498
/**
497499
* @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
{
@@ -226,6 +228,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
226228
*/
227229
Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd);
228230

231+
/**
232+
* @brief Create the control command horizon message.
233+
* @param ctrl_cmd_horizon Control command horizon to be created.
234+
* @return Created control command horizon.
235+
*/
236+
LateralHorizon createCtrlCmdHorizonMsg(const LateralHorizon & ctrl_cmd_horizon) const;
237+
229238
/**
230239
* @brief Publish the predicted future trajectory.
231240
* @param predicted_traj Predicted future trajectory to be published.

‎control/autoware_mpc_lateral_controller/src/mpc.cpp

+14-1
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ MPC::MPC(rclcpp::Node & node)
6565
bool MPC::calculateMPC(
6666
const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
6767
Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
68-
const std::string & qp_solver_type)
68+
LateralHorizon & ctrl_cmd_horizon, const std::string & qp_solver_type)
6969
{
7070
// since the reference trajectory does not take into account the current velocity of the ego
7171
// vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
@@ -234,6 +234,19 @@ bool MPC::calculateMPC(
234234
opt_error_array, m_param.prediction_horizon);
235235
}
236236

237+
// create LateralHorizon command
238+
ctrl_cmd_horizon.time_step_ms = prediction_dt * 1000.0;
239+
ctrl_cmd_horizon.controls.clear();
240+
ctrl_cmd_horizon.controls.push_back(ctrl_cmd);
241+
for (auto it = std::next(Uex.begin()); it != Uex.end(); ++it) {
242+
Lateral lateral{};
243+
lateral.steering_tire_angle = static_cast<float>(std::clamp(*it, -m_steer_lim, m_steer_lim));
244+
lateral.steering_tire_rotation_rate =
245+
(lateral.steering_tire_angle - ctrl_cmd_horizon.controls.back().steering_tire_angle) /
246+
m_ctrl_period;
247+
ctrl_cmd_horizon.controls.push_back(lateral);
248+
}
249+
237250
return true;
238251
}
239252

‎control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

+20-4
Original file line numberDiff line numberDiff line change
@@ -312,9 +312,10 @@ trajectory_follower::LateralOutput MpcLateralController::run(
312312
m_is_ctrl_cmd_prev_initialized = true;
313313
}
314314

315+
trajectory_follower::LateralHorizon ctrl_cmd_horizon{};
315316
const bool is_mpc_solved = m_mpc->calculateMPC(
316317
m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values,
317-
qp_solver_type_);
318+
ctrl_cmd_horizon, qp_solver_type_);
318319

319320
m_is_mpc_solved = is_mpc_solved; // for diagnostic updater
320321

@@ -341,9 +342,13 @@ trajectory_follower::LateralOutput MpcLateralController::run(
341342
publishPredictedTraj(predicted_traj);
342343
publishDebugValues(debug_values);
343344

344-
const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
345+
const auto createLateralOutput =
346+
[this](
347+
const auto & cmd, const bool is_mpc_solved,
348+
const auto & cmd_horizon) -> trajectory_follower::LateralOutput {
345349
trajectory_follower::LateralOutput output;
346350
output.control_cmd = createCtrlCmdMsg(cmd);
351+
output.control_cmd_horizon = createCtrlCmdHorizonMsg(cmd_horizon);
347352
// To be sure current steering of the vehicle is desired steering angle, we need to check
348353
// following conditions.
349354
// 1. At the last loop, mpc should be solved because command should be optimized output.
@@ -362,7 +367,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
362367
}
363368
// Use previous command value as previous raw steer command
364369
m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
365-
return createLateralOutput(m_ctrl_cmd_prev, false);
370+
return createLateralOutput(m_ctrl_cmd_prev, false, ctrl_cmd_horizon);
366371
}
367372

368373
if (!is_mpc_solved) {
@@ -371,7 +376,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
371376
}
372377

373378
m_ctrl_cmd_prev = ctrl_cmd;
374-
return createLateralOutput(ctrl_cmd, is_mpc_solved);
379+
return createLateralOutput(ctrl_cmd, is_mpc_solved, ctrl_cmd_horizon);
375380
}
376381

377382
bool MpcLateralController::isSteerConverged(const Lateral & cmd) const
@@ -502,6 +507,17 @@ Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd)
502507
return out;
503508
}
504509

510+
LateralHorizon MpcLateralController::createCtrlCmdHorizonMsg(
511+
const LateralHorizon & ctrl_cmd_horizon) const
512+
{
513+
auto out = ctrl_cmd_horizon;
514+
const auto now = clock_->now();
515+
for (auto & cmd : out.controls) {
516+
cmd.stamp = now;
517+
}
518+
return out;
519+
}
520+
505521
void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const
506522
{
507523
predicted_traj.header.stamp = clock_->now();

‎control/autoware_mpc_lateral_controller/test/test_mpc.cpp

+58-13
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "rclcpp/rclcpp.hpp"
2323

2424
#include <ament_index_cpp/get_package_share_directory.hpp>
25+
#include <autoware/trajectory_follower_base/control_horizon.hpp>
2526
#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
2627
#include <autoware_test_utils/autoware_test_utils.hpp>
2728

@@ -45,6 +46,7 @@
4546
namespace autoware::motion::control::mpc_lateral_controller
4647
{
4748

49+
using autoware::motion::control::trajectory_follower::LateralHorizon;
4850
using autoware_control_msgs::msg::Lateral;
4951
using autoware_planning_msgs::msg::Trajectory;
5052
using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -258,10 +260,14 @@ TEST_F(MPCTest, InitializeAndCalculate)
258260
Lateral ctrl_cmd;
259261
Trajectory pred_traj;
260262
Float32MultiArrayStamped diag;
263+
LateralHorizon ctrl_cmd_horizon;
261264
const auto odom = makeOdometry(pose_zero, default_velocity);
262-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
265+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
263266
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
264267
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
268+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
269+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
270+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
265271
}
266272

267273
TEST_F(MPCTest, InitializeAndCalculateRightTurn)
@@ -292,10 +298,14 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)
292298
Lateral ctrl_cmd;
293299
Trajectory pred_traj;
294300
Float32MultiArrayStamped diag;
301+
LateralHorizon ctrl_cmd_horizon;
295302
const auto odom = makeOdometry(pose_zero, default_velocity);
296-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
303+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
297304
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
298305
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
306+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
307+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
308+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
299309
}
300310

301311
TEST_F(MPCTest, OsqpCalculate)
@@ -321,10 +331,14 @@ TEST_F(MPCTest, OsqpCalculate)
321331
Lateral ctrl_cmd;
322332
Trajectory pred_traj;
323333
Float32MultiArrayStamped diag;
334+
LateralHorizon ctrl_cmd_horizon;
324335
const auto odom = makeOdometry(pose_zero, default_velocity);
325-
EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
336+
EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
326337
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
327338
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
339+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
340+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
341+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
328342
}
329343

330344
TEST_F(MPCTest, OsqpCalculateRightTurn)
@@ -351,10 +365,14 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
351365
Lateral ctrl_cmd;
352366
Trajectory pred_traj;
353367
Float32MultiArrayStamped diag;
368+
LateralHorizon ctrl_cmd_horizon;
354369
const auto odom = makeOdometry(pose_zero, default_velocity);
355-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
370+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
356371
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
357372
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
373+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
374+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
375+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
358376
}
359377

360378
TEST_F(MPCTest, KinematicsNoDelayCalculate)
@@ -382,10 +400,14 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
382400
Lateral ctrl_cmd;
383401
Trajectory pred_traj;
384402
Float32MultiArrayStamped diag;
403+
LateralHorizon ctrl_cmd_horizon;
385404
const auto odom = makeOdometry(pose_zero, default_velocity);
386-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
405+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
387406
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
388407
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
408+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
409+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
410+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
389411
}
390412

391413
TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
@@ -414,10 +436,14 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
414436
Lateral ctrl_cmd;
415437
Trajectory pred_traj;
416438
Float32MultiArrayStamped diag;
439+
LateralHorizon ctrl_cmd_horizon;
417440
const auto odom = makeOdometry(pose_zero, default_velocity);
418-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
441+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
419442
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
420443
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
444+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
445+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
446+
EXPECT_LT(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
421447
}
422448

423449
TEST_F(MPCTest, DynamicCalculate)
@@ -441,10 +467,14 @@ TEST_F(MPCTest, DynamicCalculate)
441467
Lateral ctrl_cmd;
442468
Trajectory pred_traj;
443469
Float32MultiArrayStamped diag;
470+
LateralHorizon ctrl_cmd_horizon;
444471
const auto odom = makeOdometry(pose_zero, default_velocity);
445-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
472+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
446473
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
447474
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
475+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
476+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
477+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
448478
}
449479

450480
TEST_F(MPCTest, MultiSolveWithBuffer)
@@ -467,24 +497,37 @@ TEST_F(MPCTest, MultiSolveWithBuffer)
467497
Lateral ctrl_cmd;
468498
Trajectory pred_traj;
469499
Float32MultiArrayStamped diag;
500+
LateralHorizon ctrl_cmd_horizon;
470501
const auto odom = makeOdometry(pose_zero, default_velocity);
471502

472-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
503+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
473504
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
474505
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
475506
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
476-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
507+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
508+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
509+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
510+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
477511
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
478512
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
479513
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
480-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
514+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
515+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
516+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
517+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
481518
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
482519
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
483520
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
484-
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
521+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
522+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
523+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
524+
ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
485525
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
486526
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
487527
EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3));
528+
EXPECT_EQ(ctrl_cmd_horizon.controls.size(), param.prediction_horizon);
529+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
530+
EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
488531
}
489532

490533
TEST_F(MPCTest, FailureCases)
@@ -508,11 +551,13 @@ TEST_F(MPCTest, FailureCases)
508551
Lateral ctrl_cmd;
509552
Trajectory pred_traj;
510553
Float32MultiArrayStamped diag;
554+
LateralHorizon ctrl_cmd_horizon;
511555
const auto odom = makeOdometry(pose_far, default_velocity);
512-
EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
556+
EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon));
513557

514558
// Calculate MPC with a fast velocity to make the prediction go further than the reference path
515559
EXPECT_FALSE(mpc->calculateMPC(
516-
neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag));
560+
neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag,
561+
ctrl_cmd_horizon));
517562
}
518563
} // 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)