From a005dd8ad5964d9bf34e20e59845c1778ce75f9b Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 13 May 2024 16:27:43 +0900 Subject: [PATCH 01/13] mpc fail checker diagnostic added Signed-off-by: Zhe Shen --- .../mpc_lateral_controller.hpp | 6 +++++ .../src/mpc_lateral_controller.cpp | 26 +++++++++++++++++++ 2 files changed, 32 insertions(+) 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 1f16bbc39bbbf..5c2331cd45372 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 @@ -90,6 +90,12 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; + Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. + + void setStatus(DiagnosticStatusWrapper & stat, const bool & m_MPC_failed); + + void setupDiag(); + std::unique_ptr m_mpc; // MPC object for trajectory following. // Check is mpc output converged 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 6aedb74e87c83..1221577e18a22 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -152,6 +152,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_mpc->setLogger(logger_); m_mpc->setClock(clock_); + + setupDiag(); } MpcLateralController::~MpcLateralController() @@ -227,6 +229,28 @@ std::shared_ptr MpcLateralController::createSteerOffset return steering_offset_; } +void MpcLateralController::setStatus( + DiagnosticStatusWrapper & stat, const bool & is_mpc_solved) +{ + if (is_mpc_solved) { + stat.summary(DiagnosticStatus::OK, "MPC succeeded."); + } else { + const std::string & error_msg = "The MPC solver failed. Call MRM to stop the car."; + stat.summary(DiagnosticStatus::ERROR, error_msg); + } +} + +void MpcLateralController::setupDiag() +{ + auto & d = diag_updater_; + d.setHardwareID("mpc_lateral_controller"); + + d.add("MPC_solve_checker", [&](auto & stat) { + setStatus( + stat, is_mpc_solved); + }); +} + trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) { @@ -255,6 +279,8 @@ trajectory_follower::LateralOutput MpcLateralController::run( const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); + diag_updater_.force_update(); + // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and // the vehicle will return to the path by re-planning the trajectory or external operation. From 1fa2de2d58d70100aa1d49b76da797e3fc207033 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 14 May 2024 16:46:24 +0900 Subject: [PATCH 02/13] fix some scope issues Signed-off-by: Zhe Shen --- .../mpc_lateral_controller/mpc_lateral_controller.hpp | 5 +++-- .../src/mpc_lateral_controller.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) 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 5c2331cd45372..c25851377f60a 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 @@ -31,6 +31,7 @@ #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include #include #include #include @@ -90,9 +91,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. + diagnostic_updater::Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. - void setStatus(DiagnosticStatusWrapper & stat, const bool & m_MPC_failed); + void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_MPC_failed); void setupDiag(); 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 1221577e18a22..bc8b9eb00da4c 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -230,13 +230,13 @@ std::shared_ptr MpcLateralController::createSteerOffset } void MpcLateralController::setStatus( - DiagnosticStatusWrapper & stat, const bool & is_mpc_solved) + diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & is_mpc_solved) { if (is_mpc_solved) { - stat.summary(DiagnosticStatus::OK, "MPC succeeded."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded."); } else { const std::string & error_msg = "The MPC solver failed. Call MRM to stop the car."; - stat.summary(DiagnosticStatus::ERROR, error_msg); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, error_msg); } } From 7b7151074cde4e778a6b4c5179e5d1fa2694e906 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 15 May 2024 10:49:11 +0900 Subject: [PATCH 03/13] member attribute added. Signed-off-by: Zhe Shen --- .../mpc_lateral_controller/mpc_lateral_controller.hpp | 5 ++++- .../src/mpc_lateral_controller.cpp | 8 +++++--- 2 files changed, 9 insertions(+), 4 deletions(-) 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 c25851377f60a..197eb4e49eca0 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 @@ -88,12 +88,15 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // Flag indicating whether to keep the steering control until it converges. bool m_keep_steer_control_until_converged; + // MPC colver checker. + bool m_is_mpc_solved{true}; + // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; diagnostic_updater::Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. - void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_MPC_failed); + void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved); void setupDiag(); 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 bc8b9eb00da4c..3075b1bbf5f1d 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -230,9 +230,9 @@ std::shared_ptr MpcLateralController::createSteerOffset } void MpcLateralController::setStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & is_mpc_solved) + diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved) { - if (is_mpc_solved) { + if (m_is_mpc_solved) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded."); } else { const std::string & error_msg = "The MPC solver failed. Call MRM to stop the car."; @@ -247,7 +247,7 @@ void MpcLateralController::setupDiag() d.add("MPC_solve_checker", [&](auto & stat) { setStatus( - stat, is_mpc_solved); + stat, m_is_mpc_solved); }); } @@ -279,6 +279,8 @@ trajectory_follower::LateralOutput MpcLateralController::run( const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); + m_is_mpc_solved = is_mpc_solved; // for diagnostic updater + diag_updater_.force_update(); // reset previous MPC result From dca5c22fce8ee4d03f6c1c8ae0dcb1cf542d9151 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 15 May 2024 13:46:04 +0900 Subject: [PATCH 04/13] shared pointer added. Signed-off-by: Zhe Shen --- .../mpc_lateral_controller/mpc_lateral_controller.hpp | 4 +--- .../src/mpc_lateral_controller.cpp | 2 +- .../autoware/trajectory_follower_node/controller_node.hpp | 2 ++ 3 files changed, 4 insertions(+), 4 deletions(-) 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 197eb4e49eca0..71237368a8ed7 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 @@ -53,7 +53,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase { public: /// \param node Reference to the node used only for the component and parameter initialization. - explicit MpcLateralController(rclcpp::Node & node); + explicit MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater_); virtual ~MpcLateralController(); private: @@ -94,8 +94,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - diagnostic_updater::Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. - void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved); void setupDiag(); 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 3075b1bbf5f1d..9f98c3761f681 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -35,7 +35,7 @@ namespace autoware::motion::control::mpc_lateral_controller { -MpcLateralController::MpcLateralController(rclcpp::Node & node) +MpcLateralController::MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater_) : clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller")) { const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; 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 06873c31cc6cc..936d506f9c208 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 @@ -73,6 +73,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node double timeout_thr_sec_; boost::optional longitudinal_output_{boost::none}; + std::shared_ptr diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. + std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; From f8e5c0e21bc1aff43231a5be5ecb096ab0e546ec Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 20 May 2024 11:38:10 +0900 Subject: [PATCH 05/13] member attribute (diag_updater_) added Signed-off-by: Zhe Shen --- .../mpc_lateral_controller/mpc_lateral_controller.hpp | 4 +++- .../src/mpc_lateral_controller.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) 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 71237368a8ed7..c6e1793d1f7f4 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 @@ -53,7 +53,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase { public: /// \param node Reference to the node used only for the component and parameter initialization. - explicit MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater_); + explicit MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater); virtual ~MpcLateralController(); private: @@ -64,6 +64,8 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase rclcpp::Publisher::SharedPtr m_pub_debug_values; rclcpp::Publisher::SharedPtr m_pub_steer_offset; + std::shared_ptr diag_updater_{}; // Diagnostic updater for publishing diagnostic data. + //!< @brief parameters for path smoothing TrajectoryFilteringParam m_trajectory_filtering_param; 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 9f98c3761f681..2d0733386d921 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -35,13 +35,15 @@ namespace autoware::motion::control::mpc_lateral_controller { -MpcLateralController::MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater_) +MpcLateralController::MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater) : clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller")) { const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; + diag_updater_ = diag_updater; + m_mpc = std::make_unique(node); m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double(); From 565b6e06711172835647b57fbe2e9e46b2e8073c Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 22 May 2024 13:40:10 +0900 Subject: [PATCH 06/13] dependency added. Signed-off-by: Zhe Shen --- .../src/mpc_lateral_controller.cpp | 6 +++--- .../autoware/trajectory_follower_node/controller_node.hpp | 7 ++++++- 2 files changed, 9 insertions(+), 4 deletions(-) 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 2d0733386d921..0e572b55316b1 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -245,9 +245,9 @@ void MpcLateralController::setStatus( void MpcLateralController::setupDiag() { auto & d = diag_updater_; - d.setHardwareID("mpc_lateral_controller"); + d->setHardwareID("mpc_lateral_controller"); - d.add("MPC_solve_checker", [&](auto & stat) { + d->add("MPC_solve_checker", [&](auto & stat) { setStatus( stat, m_is_mpc_solved); }); @@ -283,7 +283,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_mpc_solved = is_mpc_solved; // for diagnostic updater - diag_updater_.force_update(); + diag_updater_->force_update(); // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and 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 936d506f9c208..7a97c171330c5 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 @@ -29,7 +29,12 @@ #include #include +<<<<<<< HEAD #include +======= +#include +#include +>>>>>>> 7d50bb6e8 (dependency added.) #include "autoware_control_msgs/msg/control.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" @@ -73,7 +78,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node double timeout_thr_sec_; boost::optional longitudinal_output_{boost::none}; - std::shared_ptr diag_updater_{this}; // Diagnostic updater for publishing diagnostic data. + std::shared_ptr diag_updater_ = std::make_shared(this); // Diagnostic updater for publishing diagnostic data. std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; From 6b1ed8c0894d76116738bfd3bbd2b4f80a64b8b0 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 22 May 2024 13:45:53 +0900 Subject: [PATCH 07/13] implementation of the MpcLateralController corrected! Signed-off-by: Zhe Shen --- .../autoware_trajectory_follower_node/src/controller_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index 7876a38247bc9..e77e238cdaace 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -39,7 +39,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control getLateralControllerMode(declare_parameter("lateral_controller_mode")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { - lateral_controller_ = std::make_shared(*this); + lateral_controller_ = std::make_shared(*this, diag_updater_); break; } case LateralControllerMode::PURE_PURSUIT: { From d8775e4e2492011490555f8620511bf3cf71b065 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 24 May 2024 13:46:33 +0900 Subject: [PATCH 08/13] typo in comment corrected! Signed-off-by: Zhe Shen --- .../autoware/mpc_lateral_controller/mpc_lateral_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c6e1793d1f7f4..206828a2dcf7c 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 @@ -90,7 +90,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // Flag indicating whether to keep the steering control until it converges. bool m_keep_steer_control_until_converged; - // MPC colver checker. + // MPC solver checker. bool m_is_mpc_solved{true}; // trajectory buffer for detecting new trajectory From 15e88c64169fb8cbf65a71588bcd42e025cb47fd Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 27 May 2024 13:58:24 +0900 Subject: [PATCH 09/13] member method argument corrected Signed-off-by: Zhe Shen --- .../mpc_lateral_controller/mpc_lateral_controller.hpp | 2 +- .../src/mpc_lateral_controller.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 206828a2dcf7c..67dfafa4acb88 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 @@ -96,7 +96,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved); + void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); void setupDiag(); 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 0e572b55316b1..c54b473d1e9b3 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -232,7 +232,7 @@ std::shared_ptr MpcLateralController::createSteerOffset } void MpcLateralController::setStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved) + diagnostic_updater::DiagnosticStatusWrapper & stat) { if (m_is_mpc_solved) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded."); @@ -249,7 +249,7 @@ void MpcLateralController::setupDiag() d->add("MPC_solve_checker", [&](auto & stat) { setStatus( - stat, m_is_mpc_solved); + stat); }); } From 2f13bfc0d8a0a6ce74a67646db2751fd59ce9945 Mon Sep 17 00:00:00 2001 From: Zhe Shen <80969277+HansOersted@users.noreply.github.com> Date: Fri, 7 Jun 2024 14:17:07 +0900 Subject: [PATCH 10/13] delete unnecessary reference mark Co-authored-by: Takamasa Horibe --- .../src/mpc_lateral_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c54b473d1e9b3..2483a2965524a 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -237,7 +237,7 @@ void MpcLateralController::setStatus( if (m_is_mpc_solved) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded."); } else { - const std::string & error_msg = "The MPC solver failed. Call MRM to stop the car."; + const std::string error_msg = "The MPC solver failed. Call MRM to stop the car."; stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, error_msg); } } From 988b55b4c8f2c906e55cfee1aebd0d49dd20ec8a Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 19 Jun 2024 13:03:05 +0900 Subject: [PATCH 11/13] rebase Signed-off-by: Zhe Shen --- .../autoware/trajectory_follower_node/controller_node.hpp | 4 ---- 1 file changed, 4 deletions(-) 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 7a97c171330c5..56de66d3940a8 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 @@ -29,12 +29,8 @@ #include #include -<<<<<<< HEAD -#include -======= #include #include ->>>>>>> 7d50bb6e8 (dependency added.) #include "autoware_control_msgs/msg/control.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" From e84dc8cf4f07bfa3ffb48c364b5ebbbd17209738 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 19 Jun 2024 13:15:04 +0900 Subject: [PATCH 12/13] correct the include Signed-off-by: Zhe Shen --- .../autoware/trajectory_follower_node/controller_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 56de66d3940a8..9da4c06ac5a0b 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 @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include "autoware_control_msgs/msg/control.hpp" From df611892093789276f224c6c356deb867a9874ea Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 19 Jun 2024 14:03:37 +0900 Subject: [PATCH 13/13] pre-commit Signed-off-by: Zhe Shen --- .../mpc_lateral_controller.hpp | 9 ++++++--- .../src/mpc_lateral_controller.cpp | 13 +++++-------- .../trajectory_follower_node/controller_node.hpp | 4 +++- .../src/controller_node.cpp | 3 ++- 4 files changed, 16 insertions(+), 13 deletions(-) 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 67dfafa4acb88..09399d1fa2dce 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,8 @@ #include "autoware/trajectory_follower_base/lateral_controller_base.hpp" #include "rclcpp/rclcpp.hpp" +#include + #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" @@ -31,7 +33,6 @@ #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include #include #include #include @@ -53,7 +54,8 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase { public: /// \param node Reference to the node used only for the component and parameter initialization. - explicit MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater); + explicit MpcLateralController( + rclcpp::Node & node, std::shared_ptr diag_updater); virtual ~MpcLateralController(); private: @@ -64,7 +66,8 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase rclcpp::Publisher::SharedPtr m_pub_debug_values; rclcpp::Publisher::SharedPtr m_pub_steer_offset; - std::shared_ptr diag_updater_{}; // Diagnostic updater for publishing diagnostic data. + std::shared_ptr + diag_updater_{}; // Diagnostic updater for publishing diagnostic data. //!< @brief parameters for path smoothing TrajectoryFilteringParam m_trajectory_filtering_param; 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 2483a2965524a..0661b439a3982 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -35,7 +35,8 @@ namespace autoware::motion::control::mpc_lateral_controller { -MpcLateralController::MpcLateralController(rclcpp::Node & node, std::shared_ptr diag_updater) +MpcLateralController::MpcLateralController( + rclcpp::Node & node, std::shared_ptr diag_updater) : clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller")) { const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; @@ -231,8 +232,7 @@ std::shared_ptr MpcLateralController::createSteerOffset return steering_offset_; } -void MpcLateralController::setStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat) +void MpcLateralController::setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) { if (m_is_mpc_solved) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded."); @@ -247,10 +247,7 @@ void MpcLateralController::setupDiag() auto & d = diag_updater_; d->setHardwareID("mpc_lateral_controller"); - d->add("MPC_solve_checker", [&](auto & stat) { - setStatus( - stat); - }); + d->add("MPC_solve_checker", [&](auto & stat) { setStatus(stat); }); } trajectory_follower::LateralOutput MpcLateralController::run( @@ -281,7 +278,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); - m_is_mpc_solved = is_mpc_solved; // for diagnostic updater + m_is_mpc_solved = is_mpc_solved; // for diagnostic updater diag_updater_->force_update(); 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 9da4c06ac5a0b..21d58748d31fa 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 @@ -74,7 +74,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node double timeout_thr_sec_; boost::optional longitudinal_output_{boost::none}; - std::shared_ptr diag_updater_ = std::make_shared(this); // Diagnostic updater for publishing diagnostic data. + std::shared_ptr diag_updater_ = + std::make_shared( + this); // Diagnostic updater for publishing diagnostic data. std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index e77e238cdaace..274da6e9a5534 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -39,7 +39,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control getLateralControllerMode(declare_parameter("lateral_controller_mode")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { - lateral_controller_ = std::make_shared(*this, diag_updater_); + lateral_controller_ = + std::make_shared(*this, diag_updater_); break; } case LateralControllerMode::PURE_PURSUIT: {