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 vehicle_footprints{}; @@ -138,6 +139,10 @@ class LaneDepartureChecker Param param_; std::shared_ptr 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 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( "deviation/lateral", deviation.lateral); debug_publisher_.publish("deviation/yaw", deviation.yaw); debug_publisher_.publish( "deviation/yaw_deg", rad2deg(deviation.yaw)); + debug_publisher_.publish( + "deviation/longitudinal", deviation.longitudinal); + debug_publisher_.publish( + "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 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