diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
index be2411cd3268b..4eee39318f3f3 100644
--- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
+++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
@@ -91,6 +91,7 @@ struct Output
bool is_out_of_lane{};
bool will_cross_boundary{};
PoseDeviation trajectory_deviation{};
+ double deviation_longitudinal_vel{0.0};
lanelet::ConstLanelets candidate_lanelets{};
TrajectoryPoints resampled_trajectory{};
std::vector<LinearRing2d> vehicle_footprints{};
@@ -138,6 +139,10 @@ class LaneDepartureChecker
Param param_;
std::shared_ptr<vehicle_info_util::VehicleInfo> vehicle_info_ptr_;
+ static double calcLongitudinalDeviationDerivatives(
+ const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold,
+ const Input & input);
+
static PoseDeviation calcTrajectoryDeviation(
const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose,
const double dist_threshold, const double yaw_threshold);
diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
index 811e1652fcb4a..2f9c80fa1c7d2 100644
--- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
+++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
@@ -104,6 +104,10 @@ Output LaneDepartureChecker::update(const Input & input)
tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+ output.deviation_longitudinal_vel = calcLongitudinalDeviationDerivatives(
+ *input.predicted_trajectory, param_.ego_nearest_dist_threshold,
+ param_.ego_nearest_yaw_threshold, input);
+
output.trajectory_deviation = calcTrajectoryDeviation(
*input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold,
param_.ego_nearest_yaw_threshold);
@@ -165,6 +169,23 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
return willLeaveLane(candidate_lanelets, vehicle_footprints);
}
+double LaneDepartureChecker::calcLongitudinalDeviationDerivatives(
+ const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold,
+ const Input & input)
+{
+ const geometry_msgs::msg::Pose & pose = input.current_odom->pose.pose;
+ const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+ trajectory.points, pose, dist_threshold, yaw_threshold);
+ double deviation_longitudinal_vel;
+ const auto & ref_point = trajectory.points.at(nearest_idx);
+
+ const auto current_vel = input.current_odom->twist.twist.linear.x;
+ const double ref_vel = ref_point.longitudinal_velocity_mps;
+ deviation_longitudinal_vel = current_vel - ref_vel;
+
+ return deviation_longitudinal_vel;
+}
+
PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
const double yaw_threshold)
diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
index 06d11133920f4..2c40dd17371ff 100644
--- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
+++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
@@ -358,11 +358,16 @@ void LaneDepartureCheckerNode::onTimer()
{
const auto & deviation = output_.trajectory_deviation;
+ const double deviation_vel_longitudinal = output_.deviation_longitudinal_vel;
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
"deviation/lateral", deviation.lateral);
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>("deviation/yaw", deviation.yaw);
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
"deviation/yaw_deg", rad2deg(deviation.yaw));
+ debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
+ "deviation/longitudinal", deviation.longitudinal);
+ debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
+ "deviation/longitudinal_velocity", deviation_vel_longitudinal);
}
processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true);
diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md
index 6bfba5818bd06..2bb1a7c8a2d55 100644
--- a/control/mpc_lateral_controller/README.md
+++ b/control/mpc_lateral_controller/README.md
@@ -202,6 +202,32 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as
| cf | double | front cornering power [N/rad] | 155494.663 |
| cr | double | rear cornering power [N/rad] | 155494.663 |
+### Visualizations of the control results (debug information)
+
+Some signals are published as topics to evaluate the quality of the control results. A control researcher can visualize these information using the plotting tool such as PlotJuggler. Note that the 'deviation' and the 'state error' are defined as the difference between the state and the reference. That is state - reference. This definition is different from the conventional sign of the 'state error' defined in the control theory.
+
+`/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal`:
+This topic represents the longitudinal deviation of the position at the current time.
+
+`/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal_velocity`:
+This topic represents the longitudinal deviation of the velocity at the current time.
+
+`/control/trajectory_follower/lateral/diagnostic`:
+The signals in this topic evaluate the quality of MPC.
+
+Some commonly used signals are listed below:
+
+| The signal | Number |
+| :------------------------------------------------------------- | :----- |
+| final steering command (MPC + Clamp + LPF) | [0] |
+| MPC result | [1] |
+| MPC after Clamp (MPC + Clamp) | [21] |
+| feedforward term (reference steering angle) | [2] |
+| lateral error w.r.t the modified reference, feeding MPC module | [5] |
+| lateral error w.r.t the original reference, feeding mpc.cpp | [22] |
+
+Note: The default error output for each signal is -1.
+
### How to tune MPC parameters
#### Set kinematics information
diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp
index 2e83c5ab847c4..87008701b4881 100644
--- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp
+++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp
@@ -377,9 +377,10 @@ class MPC
* @return The generated diagnostic data.
*/
Float32MultiArrayStamped generateDiagData(
- const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
- const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
- const Odometry & current_kinematics) const;
+ const MPCTrajectory & reference_trajectory, const MPCData & mpc_data_traj_raw,
+ const MPCData & mpc_data, const MPCMatrix & mpc_matrix,
+ const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
+ const Odometry & current_kinematics, bool success_data_traj_raw) const;
/**
* @brief calculate steering rate limit along with the target trajectory
@@ -406,6 +407,7 @@ class MPC
public:
MPCTrajectory m_reference_trajectory; // Reference trajectory to be followed.
+ MPCTrajectory m_mpc_traj_raw; // The raw trajectory from the planner.
MPCParam m_param; // MPC design parameters.
std::deque<double> m_input_buffer; // MPC output buffer for delay time compensation.
double m_raw_steer_cmd_prev = 0.0; // Previous MPC raw output.
diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp
index 8fa95f4b3b87b..4f0f82f80e093 100644
--- a/control/mpc_lateral_controller/src/mpc.cpp
+++ b/control/mpc_lateral_controller/src/mpc.cpp
@@ -73,6 +73,10 @@ bool MPC::calculateMPC(
return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
}
+ // get the diagnostic data w.r.t. the original trajectory
+ const auto [success_data_traj_raw, mpc_data_traj_raw] =
+ getData(m_mpc_traj_raw, current_steer, current_kinematics);
+
// generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
@@ -109,16 +113,17 @@ bool MPC::calculateMPC(
calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt);
// prepare diagnostic message
- diagnostic =
- generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
+ diagnostic = generateDiagData(
+ reference_trajectory, mpc_data_traj_raw, mpc_data, mpc_matrix, ctrl_cmd, Uex,
+ current_kinematics, success_data_traj_raw);
return true;
}
Float32MultiArrayStamped MPC::generateDiagData(
- const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
- const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
- const Odometry & current_kinematics) const
+ const MPCTrajectory & reference_trajectory, const MPCData & mpc_data_traj_raw,
+ const MPCData & mpc_data, const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd,
+ const VectorXd & Uex, const Odometry & current_kinematics, bool success_data_traj_raw) const
{
Float32MultiArrayStamped diagnostic;
@@ -143,7 +148,7 @@ Float32MultiArrayStamped MPC::generateDiagData(
append_diag(mpc_matrix.Uref_ex(0)); // [2] feed-forward steering value
append_diag(std::atan(nearest_smooth_k * wb)); // [3] feed-forward steering value raw
append_diag(mpc_data.steer); // [4] current steering angle
- append_diag(mpc_data.lateral_err); // [5] lateral error
+ append_diag(mpc_data.lateral_err); // [5] lateral error (the actual error used for MPC)
append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation)); // [6] current_pose yaw
append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation)); // [7] nearest_pose yaw
append_diag(mpc_data.yaw_err); // [8] yaw error
@@ -159,6 +164,12 @@ Float32MultiArrayStamped MPC::generateDiagData(
append_diag(iteration_num); // [18] iteration number
append_diag(runtime); // [19] runtime of the latest problem solved
append_diag(objective_value); // [20] objective value of the latest problem solved
+ append_diag(std::clamp(
+ Uex(0), -m_steer_lim,
+ m_steer_lim)); // [21] control signal after the saturation constraint (clamp)
+ append_diag(
+ success_data_traj_raw ? mpc_data_traj_raw.lateral_err
+ : -1.0); // [22] lateral error from raw trajectory
return diagnostic;
}
@@ -173,11 +184,11 @@ void MPC::setReferenceTrajectory(
const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
- const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
+ m_mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
// resampling
const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
- mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
+ m_mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
if (!success_resample) {
warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
return;
@@ -214,7 +225,7 @@ void MPC::setReferenceTrajectory(
*/
if (param.extend_trajectory_for_end_yaw_control) {
MPCUtils::extendTrajectoryInYawDirection(
- mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+ m_mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
}
// calculate yaw angle