Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(mpc_lateral_controller): signal a MRM when MPC fails. #7016

Merged
merged 13 commits into from
Jun 19, 2024
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#include "autoware/trajectory_follower_base/lateral_controller_base.hpp"
#include "rclcpp/rclcpp.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>

#include "autoware_control_msgs/msg/lateral.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
Expand Down Expand Up @@ -52,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);
explicit MpcLateralController(
rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater);
virtual ~MpcLateralController();

private:
Expand All @@ -63,6 +66,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr m_pub_debug_values;
rclcpp::Publisher<Float32Stamped>::SharedPtr m_pub_steer_offset;

std::shared_ptr<diagnostic_updater::Updater>
diag_updater_{}; // Diagnostic updater for publishing diagnostic data.

//!< @brief parameters for path smoothing
TrajectoryFilteringParam m_trajectory_filtering_param;

Expand All @@ -87,9 +93,16 @@ 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 solver checker.
bool m_is_mpc_solved{true};

// trajectory buffer for detecting new trajectory
std::deque<Trajectory> m_trajectory_buffer;

void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);

void setupDiag();

std::unique_ptr<MPC> m_mpc; // MPC object for trajectory following.

// Check is mpc output converged
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,13 +35,16 @@
namespace autoware::motion::control::mpc_lateral_controller
{

MpcLateralController::MpcLateralController(rclcpp::Node & node)
MpcLateralController::MpcLateralController(
rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> 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<int>(s); };
const auto dp_bool = [&](const std::string & s) { return node.declare_parameter<bool>(s); };
const auto dp_double = [&](const std::string & s) { return node.declare_parameter<double>(s); };

diag_updater_ = diag_updater;

m_mpc = std::make_unique<MPC>(node);

m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double();
Expand Down Expand Up @@ -152,6 +155,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)

m_mpc->setLogger(logger_);
m_mpc->setClock(clock_);

setupDiag();
}

MpcLateralController::~MpcLateralController()
Expand Down Expand Up @@ -227,6 +232,24 @@ std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffset
return steering_offset_;
}

void MpcLateralController::setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
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.";
stat.summary(diagnostic_msgs::msg::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); });
}

trajectory_follower::LateralOutput MpcLateralController::run(
trajectory_follower::InputData const & input_data)
{
Expand Down Expand Up @@ -255,6 +278,10 @@ 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
// 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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <autoware/universe_utils/ros/published_time_publisher.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>

#include "autoware_control_msgs/msg/control.hpp"
#include "autoware_control_msgs/msg/longitudinal.hpp"
Expand Down Expand Up @@ -73,6 +74,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
double timeout_thr_sec_;
boost::optional<LongitudinalOutput> longitudinal_output_{boost::none};

std::shared_ptr<diagnostic_updater::Updater> diag_updater_ =
std::make_shared<diagnostic_updater::Updater>(
this); // Diagnostic updater for publishing diagnostic data.

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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
getLateralControllerMode(declare_parameter<std::string>("lateral_controller_mode"));
switch (lateral_controller_mode) {
case LateralControllerMode::MPC: {
lateral_controller_ = std::make_shared<mpc_lateral_controller::MpcLateralController>(*this);
lateral_controller_ =
std::make_shared<mpc_lateral_controller::MpcLateralController>(*this, diag_updater_);
break;
}
case LateralControllerMode::PURE_PURSUIT: {
Expand Down
Loading