Skip to content

Commit 91f78d1

Browse files
committed
dependency added.
Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
1 parent 689f7ad commit 91f78d1

File tree

2 files changed

+5
-4
lines changed

2 files changed

+5
-4
lines changed

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -245,9 +245,9 @@ void MpcLateralController::setStatus(
245245
void MpcLateralController::setupDiag()
246246
{
247247
auto & d = diag_updater_;
248-
d.setHardwareID("mpc_lateral_controller");
248+
d->setHardwareID("mpc_lateral_controller");
249249

250-
d.add("MPC_solve_checker", [&](auto & stat) {
250+
d->add("MPC_solve_checker", [&](auto & stat) {
251251
setStatus(
252252
stat, m_is_mpc_solved);
253253
});
@@ -283,7 +283,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
283283

284284
m_is_mpc_solved = is_mpc_solved; // for diagnostic updater
285285

286-
diag_updater_.force_update();
286+
diag_updater_->force_update();
287287

288288
// reset previous MPC result
289289
// Note: When a large deviation from the trajectory occurs, the optimization stops and

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <Eigen/Core>
3030
#include <Eigen/Geometry>
3131
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
32+
#include <diagnostic_updater/diagnostic_updater.hpp>
3233

3334
#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
3435
#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
@@ -73,7 +74,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
7374
double timeout_thr_sec_;
7475
boost::optional<LongitudinalOutput> longitudinal_output_{boost::none};
7576

76-
std::shared_ptr<diagnostic_updater::Updater> diag_updater_{this}; // Diagnostic updater for publishing diagnostic data.
77+
std::shared_ptr<diagnostic_updater::Updater> diag_updater_ = std::make_shared<diagnostic_updater::Updater>(this); // Diagnostic updater for publishing diagnostic data.
7778

7879
std::shared_ptr<trajectory_follower::LongitudinalControllerBase> longitudinal_controller_;
7980
std::shared_ptr<trajectory_follower::LateralControllerBase> lateral_controller_;

0 commit comments

Comments
 (0)