From c1e0177b3ab9513fb8fc70775e29ee559c68e5bf Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 22 Jan 2024 12:27:04 +0900 Subject: [PATCH 01/23] lingitudinal deviation published, time stamp is CURRENT. Signed-off-by: Zhe Shen --- .../lane_departure_checker_node/lane_departure_checker_node.cpp | 2 ++ 1 file changed, 2 insertions(+) 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..ed3ca610a8eb6 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 @@ -363,6 +363,8 @@ void LaneDepartureCheckerNode::onTimer() debug_publisher_.publish("deviation/yaw", deviation.yaw); debug_publisher_.publish( "deviation/yaw_deg", rad2deg(deviation.yaw)); + debug_publisher_.publish( + "deviation/longitudinal", deviation.longitudinal); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); From b7d6251103cdfc6f4af5edb0d1f80fc408f7fe0f Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 22 Jan 2024 18:05:15 +0900 Subject: [PATCH 02/23] add the derivative feature: structure defined, Output modified, derivative function prototype defined Signed-off-by: Zhe Shen --- .../lane_departure_checker.hpp | 12 ++++++++++++ .../lane_departure_checker.cpp | 4 ++++ .../lane_departure_checker_node.cpp | 4 ++++ 3 files changed, 20 insertions(+) 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 dc55576640133..6a22dbf7a9795 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 @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -78,6 +79,12 @@ struct Input std::vector boundary_types_to_detect{}; }; +struct DerivativeDeviation +{ + double longitudinal_vel{0.0}; + double longitudinal_acc{0.0}; +}; + struct Output { std::map processing_time_map{}; @@ -85,6 +92,7 @@ struct Output bool is_out_of_lane{}; bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; + DerivativeDeviation trajectory_der_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; std::vector vehicle_footprints{}; @@ -119,6 +127,10 @@ class LaneDepartureChecker Param param_; std::shared_ptr vehicle_info_ptr_; + static DerivativeDeviation calcLongitudinalDeviationDerivatives( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold); + 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 5bd0fcace9909..4b19e0b7b14bb 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 @@ -102,6 +102,10 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch stop_watch; + output.trajectory_der_deviation = calcLongitudinalDeviationDerivatives( + *input.predicted_trajectory, input.current_odom->twist.twist, param_.ego_nearest_dist_threshold, + param_.ego_nearest_yaw_threshold); + output.trajectory_deviation = calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, param_.ego_nearest_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 ed3ca610a8eb6..6108de0e98567 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 @@ -365,6 +365,10 @@ void LaneDepartureCheckerNode::onTimer() "deviation/yaw_deg", rad2deg(deviation.yaw)); debug_publisher_.publish( "deviation/longitudinal", deviation.longitudinal); + debug_publisher_.publish( + "deviation/longitudinal_velocity", deviation.longitudinal_vel); + debug_publisher_.publish( + "deviation/longitudinal_acceleration", deviation.longitudinal_acc); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); From ba0abd5b29b83366d4bcb267e6c57bcba59cd2dd Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 23 Jan 2024 14:22:21 +0900 Subject: [PATCH 03/23] add the velocity and acceleration frame, but the acceleration might be incorrect Signed-off-by: Zhe Shen --- .../lane_departure_checker.hpp | 1 - .../lane_departure_checker.cpp | 23 +++++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) 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 6a22dbf7a9795..3df140442232e 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 @@ -18,7 +18,6 @@ #include #include #include -#include #include #include 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 4b19e0b7b14bb..32583cb68a3b2 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 @@ -167,6 +167,29 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( return willLeaveLane(candidate_lanelets, vehicle_footprints); } +DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold) +{ + const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + trajectory.points, pose, dist_threshold, yaw_threshold); + + DerivativeDeviation der_deviation; + + const auto & point = trajectory.points.at(nearest_idx); + const auto vel = point.twist.linear.x; + const auto acc = point.accel.linear.x; + + const auto & ref_point = trajectory.points.at(nearest_idx); + const auto ref_vel = ref_point.twist.linear.x; + const auto ref_acc = ref_point.accel.linear.x; + + der_deviation.longitudinal_vel = vel - ref_vel; + der_deviation.longitudinal_acc = acc - ref_acc; + + return der_deviation; +} + PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) From fbfd214a1f7abe91dc0995810b26921022adb77c Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 23 Jan 2024 15:43:18 +0900 Subject: [PATCH 04/23] deviation of the velocity along longitude published Signed-off-by: Zhe Shen --- .../lane_departure_checker.hpp | 4 ++-- .../lane_departure_checker.cpp | 23 +++++++------------ 2 files changed, 10 insertions(+), 17 deletions(-) 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 3df140442232e..f13aeed486dc9 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 @@ -127,8 +127,8 @@ class LaneDepartureChecker std::shared_ptr vehicle_info_ptr_; static DerivativeDeviation calcLongitudinalDeviationDerivatives( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold); + 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, 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 32583cb68a3b2..8ad724e12acf5 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 @@ -103,8 +103,8 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch stop_watch; output.trajectory_der_deviation = calcLongitudinalDeviationDerivatives( - *input.predicted_trajectory, input.current_odom->twist.twist, param_.ego_nearest_dist_threshold, - param_.ego_nearest_yaw_threshold); + *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, @@ -168,24 +168,17 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( } DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) + 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); - DerivativeDeviation der_deviation; - - const auto & point = trajectory.points.at(nearest_idx); - const auto vel = point.twist.linear.x; - const auto acc = point.accel.linear.x; - + const auto current_vel = input.current_odom->twist.twist.linear.x; const auto & ref_point = trajectory.points.at(nearest_idx); - const auto ref_vel = ref_point.twist.linear.x; - const auto ref_acc = ref_point.accel.linear.x; - - der_deviation.longitudinal_vel = vel - ref_vel; - der_deviation.longitudinal_acc = acc - ref_acc; + const auto ref_vel = ref_point.longitudinal_velocity_mps; + der_deviation.longitudinal_vel = current_vel - ref_vel; return der_deviation; } From 0fb0db72b478e07effe887023cb5678cae406aa0 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 23 Jan 2024 15:50:16 +0900 Subject: [PATCH 05/23] output correct for the derivatives Signed-off-by: Zhe Shen --- .../lane_departure_checker_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 6108de0e98567..180c6679b1383 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,6 +358,7 @@ void LaneDepartureCheckerNode::onTimer() { const auto & deviation = output_.trajectory_deviation; + const auto & der_deviation = output_.trajectory_der_deviation; debug_publisher_.publish( "deviation/lateral", deviation.lateral); debug_publisher_.publish("deviation/yaw", deviation.yaw); @@ -366,9 +367,9 @@ void LaneDepartureCheckerNode::onTimer() debug_publisher_.publish( "deviation/longitudinal", deviation.longitudinal); debug_publisher_.publish( - "deviation/longitudinal_velocity", deviation.longitudinal_vel); + "deviation/longitudinal_velocity", der_deviation.longitudinal_vel); debug_publisher_.publish( - "deviation/longitudinal_acceleration", deviation.longitudinal_acc); + "deviation/longitudinal_acceleration", der_deviation.longitudinal_acc); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); From 3a28bf3ab7f21e7c6801f0f4a18a18375aca9a8e Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 23 Jan 2024 16:18:55 +0900 Subject: [PATCH 06/23] add the 'interface' for the deviation of the acceleration along longitude, but the current_acc may only be available by subscribing Signed-off-by: Zhe Shen --- .../lane_departure_checker.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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 8ad724e12acf5..6fa1b0244a004 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 @@ -175,11 +175,16 @@ DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, pose, dist_threshold, yaw_threshold); DerivativeDeviation der_deviation; - const auto current_vel = input.current_odom->twist.twist.linear.x; const auto & ref_point = trajectory.points.at(nearest_idx); - const auto ref_vel = ref_point.longitudinal_velocity_mps; + + const auto current_vel = input.current_odom->twist.twist.linear.x; + const auto & ref_vel = ref_point.longitudinal_velocity_mps; der_deviation.longitudinal_vel = current_vel - ref_vel; + const auto current_acc = 0; // There is no acc from input.current_odom->twist.twist.linear.x. You may need to subscribe. + const auto & ref_acc = ref_point.acceleration_mps2; + der_deviation.longitudinal_acc = current_acc - ref_acc; + return der_deviation; } From 156b5313049fbd26bc7e2ff6865ae19825f35e91 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Thu, 25 Jan 2024 15:37:13 +0900 Subject: [PATCH 07/23] delete the acce deviation Signed-off-by: Zhe Shen --- .../include/lane_departure_checker/lane_departure_checker.hpp | 1 - .../lane_departure_checker_node/lane_departure_checker.cpp | 4 ---- .../lane_departure_checker_node.cpp | 2 -- 3 files changed, 7 deletions(-) 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 f13aeed486dc9..985eeeb393e57 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 @@ -81,7 +81,6 @@ struct Input struct DerivativeDeviation { double longitudinal_vel{0.0}; - double longitudinal_acc{0.0}; }; struct Output 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 6fa1b0244a004..22a101d0d87f9 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 @@ -181,10 +181,6 @@ DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( const auto & ref_vel = ref_point.longitudinal_velocity_mps; der_deviation.longitudinal_vel = current_vel - ref_vel; - const auto current_acc = 0; // There is no acc from input.current_odom->twist.twist.linear.x. You may need to subscribe. - const auto & ref_acc = ref_point.acceleration_mps2; - der_deviation.longitudinal_acc = current_acc - ref_acc; - return der_deviation; } 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 180c6679b1383..5698db3d84ca6 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 @@ -368,8 +368,6 @@ void LaneDepartureCheckerNode::onTimer() "deviation/longitudinal", deviation.longitudinal); debug_publisher_.publish( "deviation/longitudinal_velocity", der_deviation.longitudinal_vel); - debug_publisher_.publish( - "deviation/longitudinal_acceleration", der_deviation.longitudinal_acc); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); From 393cbb65115cf7485a70e3559eea33443e764cc8 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 29 Jan 2024 16:52:45 +0900 Subject: [PATCH 08/23] output the debug signal, the MPC after saturation before filter in specific Signed-off-by: Zhe Shen --- control/mpc_lateral_controller/src/mpc.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8fa95f4b3b87b..3c1513be23159 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -159,6 +159,7 @@ 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) return diagnostic; } From 2819e7579238c82a2eccafa5562c8aebeb15c329 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 31 Jan 2024 15:16:00 +0900 Subject: [PATCH 09/23] correct the published lateral_deviation (it should be w.r.t. the actual reference, that is mpc_resampled_ref_trajectory). publish the mpc_data_traj_raw.lateral_err w.r.t. the raw planned path Signed-off-by: Zhe Shen --- .../include/mpc_lateral_controller/mpc.hpp | 10 +++++---- control/mpc_lateral_controller/src/mpc.cpp | 22 ++++++++++++++++--- 2 files changed, 25 insertions(+), 7 deletions(-) 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..61d7f2a5d67ce 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -376,10 +376,11 @@ class MPC * @param current_kinematics The current vehicle kinematics. * @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; +Float32MultiArrayStamped generateDiagData( + 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) 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 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 3c1513be23159..7f0acc75d9c20 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -73,6 +73,20 @@ bool MPC::calculateMPC( return fail_warn_throttle("trajectory resampling failed. Stop MPC."); } + // get the diagnostic data + const auto [success_data_for_diagnostic, mpc_data_for_diagnostic] = + getData(mpc_resampled_ref_trajectory, current_steer, current_kinematics); + if (!success_data_for_diagnostic) { + return fail_warn_throttle("fail to get MPC Data for the diagnostic. Stop MPC."); + } + + // get the diagnostic data w.r.t. the original trajectory + const auto [success_data_traj_raw, mpc_data_traj_raw] = + getData(mpc_traj_raw, current_steer, current_kinematics); + if (!success_data_traj_raw) { + return fail_warn_throttle("fail to get MPC Data for the raw trajectory. Stop MPC."); + } + // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); @@ -110,13 +124,14 @@ bool MPC::calculateMPC( // prepare diagnostic message diagnostic = - generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics); + generateDiagData(mpc_resampled_ref_trajectory, mpc_data_traj_raw, mpc_data_for_diagnostic, mpc_matrix, ctrl_cmd, Uex, current_kinematics); return true; } Float32MultiArrayStamped MPC::generateDiagData( - const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, + 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) const { @@ -160,6 +175,7 @@ Float32MultiArrayStamped MPC::generateDiagData( 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(mpc_data_traj_raw.lateral_err); // [22] lateral error from raw trajectory return diagnostic; } @@ -174,7 +190,7 @@ 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); + mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); // resampling const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance( From 3e209cdea75be47b5bf8f1d8ff894a4ff21b7de3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 1 Feb 2024 03:09:35 +0000 Subject: [PATCH 10/23] style(pre-commit): autofix --- .../lane_departure_checker.hpp | 4 ++-- .../lane_departure_checker.cpp | 8 ++++---- .../include/mpc_lateral_controller/mpc.hpp | 10 +++++----- control/mpc_lateral_controller/src/mpc.cpp | 18 ++++++++++-------- 4 files changed, 21 insertions(+), 19 deletions(-) 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 985eeeb393e57..e7eb0fff6bb6f 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 @@ -126,8 +126,8 @@ class LaneDepartureChecker std::shared_ptr vehicle_info_ptr_; static DerivativeDeviation calcLongitudinalDeviationDerivatives( - const Trajectory & trajectory, const double dist_threshold, - const double yaw_threshold, const Input & input); + 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, 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 22a101d0d87f9..60b11ee4a5a6a 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 @@ -103,7 +103,7 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch stop_watch; output.trajectory_der_deviation = calcLongitudinalDeviationDerivatives( - *input.predicted_trajectory, param_.ego_nearest_dist_threshold, + *input.predicted_trajectory, param_.ego_nearest_dist_threshold, param_.ego_nearest_yaw_threshold, input); output.trajectory_deviation = calcTrajectoryDeviation( @@ -168,8 +168,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( } DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( - const Trajectory & trajectory, const double dist_threshold, - const double yaw_threshold, const Input & input) + 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( @@ -182,7 +182,7 @@ DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( der_deviation.longitudinal_vel = current_vel - ref_vel; return der_deviation; -} +} PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, 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 61d7f2a5d67ce..e0b339dcbb5f7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -376,11 +376,11 @@ class MPC * @param current_kinematics The current vehicle kinematics. * @return The generated diagnostic data. */ -Float32MultiArrayStamped generateDiagData( - 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) const; + Float32MultiArrayStamped generateDiagData( + 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) const; /** * @brief calculate steering rate limit along with the target trajectory diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 7f0acc75d9c20..e4a323137a994 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -123,17 +123,17 @@ bool MPC::calculateMPC( calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt); // prepare diagnostic message - diagnostic = - generateDiagData(mpc_resampled_ref_trajectory, mpc_data_traj_raw, mpc_data_for_diagnostic, mpc_matrix, ctrl_cmd, Uex, current_kinematics); + diagnostic = generateDiagData( + mpc_resampled_ref_trajectory, mpc_data_traj_raw, mpc_data_for_diagnostic, mpc_matrix, ctrl_cmd, + Uex, current_kinematics); return true; } Float32MultiArrayStamped MPC::generateDiagData( - 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) 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) const { Float32MultiArrayStamped diagnostic; @@ -174,8 +174,10 @@ 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(mpc_data_traj_raw.lateral_err); // [22] lateral error from raw trajectory + append_diag(std::clamp( + Uex(0), -m_steer_lim, + m_steer_lim)); // [21] control signal after the saturation constraint (clamp) + append_diag(mpc_data_traj_raw.lateral_err); // [22] lateral error from raw trajectory return diagnostic; } From a47aae117c52982d4f918716245b9d434774cf2c Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 9 Feb 2024 14:46:58 +0900 Subject: [PATCH 11/23] add the description of the signals in the README.md Signed-off-by: Zhe Shen --- control/mpc_lateral_controller/README.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 6bfba5818bd06..e1db608e5ca5f 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -202,6 +202,29 @@ 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` +The longitudinal deviation of the position at the current time. + +`/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal_velocity` +The longitudinal deviation of the velocity at the current time. + +`/control/trajectory_follower/lateral/diagnostic` +The signals evaluating the quality of MPC. +Some commonly used signal 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] | + + ### How to tune MPC parameters #### Set kinematics information From 493d455483a8b484daa32a62a9e3d0d3c96dfbc6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 9 Feb 2024 05:48:54 +0000 Subject: [PATCH 12/23] style(pre-commit): autofix --- control/mpc_lateral_controller/README.md | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index e1db608e5ca5f..dd7ff3166351a 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -206,7 +206,7 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as 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` +`/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal` The longitudinal deviation of the position at the current time. `/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal_velocity` @@ -214,16 +214,15 @@ The longitudinal deviation of the velocity at the current time. `/control/trajectory_follower/lateral/diagnostic` The signals evaluating the quality of MPC. -Some commonly used signal are listed below: -| The signal | Number | +Some commonly used signal 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] | - +| 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] | ### How to tune MPC parameters From 6ba472aa4ea3ab000e2494328e399dc25a5eebda Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 26 Feb 2024 16:57:09 +0900 Subject: [PATCH 13/23] polish the words and the format Signed-off-by: Zhe Shen --- control/mpc_lateral_controller/README.md | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index dd7ff3166351a..ad102b4938286 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -206,15 +206,17 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as 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` -The longitudinal deviation of the position at the current time. +`/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` -The longitudinal deviation of the velocity 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. -`/control/trajectory_follower/lateral/diagnostic` -The signals evaluating the quality of MPC. Some commonly used signal are listed below: + | The signal | Number | | :------------------------------------------------------------- | :----- | | final steering command (MPC + Clamp + LPF) | [0] | From 916234865e5cb89a9d5c92ab395eadd92346833d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Feb 2024 07:59:14 +0000 Subject: [PATCH 14/23] style(pre-commit): autofix --- control/mpc_lateral_controller/README.md | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index ad102b4938286..c8dc647b97b84 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -206,25 +206,25 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as 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`: +`/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`: +`/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`: +`/control/trajectory_follower/lateral/diagnostic`: The signals in this topic evaluate the quality of MPC. Some commonly used signal are listed below: -| The signal | Number | +| 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] | +| 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] | ### How to tune MPC parameters From 4622b0e8656df65d263588e39365e96c28f35860 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 27 Feb 2024 14:26:10 +0900 Subject: [PATCH 15/23] typo fixed Signed-off-by: Zhe Shen --- control/mpc_lateral_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index c8dc647b97b84..15f3d6741d201 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -215,7 +215,7 @@ This topic represents the longitudinal deviation of the velocity at the current `/control/trajectory_follower/lateral/diagnostic`: The signals in this topic evaluate the quality of MPC. -Some commonly used signal are listed below: +Some commonly used signals are listed below: | The signal | Number | | :------------------------------------------------------------- | :----- | From 821cf29de7f95895292c453da6570cf537339121 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 1 Mar 2024 13:34:36 +0900 Subject: [PATCH 16/23] use the double type instead of the structure Signed-off-by: Zhe Shen --- .../lane_departure_checker/lane_departure_checker.hpp | 9 ++------- .../lane_departure_checker.cpp | 10 +++++----- .../lane_departure_checker_node.cpp | 4 ++-- 3 files changed, 9 insertions(+), 14 deletions(-) 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 8705d5b32a6da..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 @@ -84,11 +84,6 @@ struct Input std::vector boundary_types_to_detect{}; }; -struct DerivativeDeviation -{ - double longitudinal_vel{0.0}; -}; - struct Output { std::map processing_time_map{}; @@ -96,7 +91,7 @@ struct Output bool is_out_of_lane{}; bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; - DerivativeDeviation trajectory_der_deviation{}; + double deviation_longitudinal_vel{0.0}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; std::vector vehicle_footprints{}; @@ -144,7 +139,7 @@ class LaneDepartureChecker Param param_; std::shared_ptr vehicle_info_ptr_; - static DerivativeDeviation calcLongitudinalDeviationDerivatives( + static double calcLongitudinalDeviationDerivatives( const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold, const Input & input); 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 097da665a0ab4..eff3662f9448f 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,7 +104,7 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch stop_watch; - output.trajectory_der_deviation = calcLongitudinalDeviationDerivatives( + output.deviation_longitudinal_vel = calcLongitudinalDeviationDerivatives( *input.predicted_trajectory, param_.ego_nearest_dist_threshold, param_.ego_nearest_yaw_threshold, input); @@ -169,21 +169,21 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( return willLeaveLane(candidate_lanelets, vehicle_footprints); } -DerivativeDeviation LaneDepartureChecker::calcLongitudinalDeviationDerivatives( +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); - DerivativeDeviation der_deviation; + 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 auto & ref_vel = ref_point.longitudinal_velocity_mps; - der_deviation.longitudinal_vel = current_vel - ref_vel; + deviation_longitudinal_vel = current_vel - ref_vel; - return der_deviation; + return deviation_longitudinal_vel; } PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( 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 5698db3d84ca6..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,7 +358,7 @@ void LaneDepartureCheckerNode::onTimer() { const auto & deviation = output_.trajectory_deviation; - const auto & der_deviation = output_.trajectory_der_deviation; + const double deviation_vel_longitudinal = output_.deviation_longitudinal_vel; debug_publisher_.publish( "deviation/lateral", deviation.lateral); debug_publisher_.publish("deviation/yaw", deviation.yaw); @@ -367,7 +367,7 @@ void LaneDepartureCheckerNode::onTimer() debug_publisher_.publish( "deviation/longitudinal", deviation.longitudinal); debug_publisher_.publish( - "deviation/longitudinal_velocity", der_deviation.longitudinal_vel); + "deviation/longitudinal_velocity", deviation_vel_longitudinal); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); From 523bdfd30907e41effdae5f33228325498f70a51 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 1 Mar 2024 13:55:27 +0900 Subject: [PATCH 17/23] avoid using reference for type double Signed-off-by: Zhe Shen --- .../src/lane_departure_checker_node/lane_departure_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 eff3662f9448f..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 @@ -180,7 +180,7 @@ double LaneDepartureChecker::calcLongitudinalDeviationDerivatives( const auto & ref_point = trajectory.points.at(nearest_idx); const auto current_vel = input.current_odom->twist.twist.linear.x; - const auto & ref_vel = ref_point.longitudinal_velocity_mps; + const double ref_vel = ref_point.longitudinal_velocity_mps; deviation_longitudinal_vel = current_vel - ref_vel; return deviation_longitudinal_vel; From 1f5d521f4950c5ed40be38b00dede1a3219b6b32 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 1 Mar 2024 14:26:33 +0900 Subject: [PATCH 18/23] follow the name convention Signed-off-by: Zhe Shen --- .../include/mpc_lateral_controller/mpc.hpp | 2 +- control/mpc_lateral_controller/src/mpc.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) 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 e0b339dcbb5f7..aa965dce7a3dd 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -407,7 +407,7 @@ class MPC public: MPCTrajectory m_reference_trajectory; // Reference trajectory to be followed. - MPCTrajectory mpc_traj_raw; // The raw trajectory from the planner. + 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 e4a323137a994..ed7dcf1c14935 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -82,7 +82,7 @@ bool MPC::calculateMPC( // get the diagnostic data w.r.t. the original trajectory const auto [success_data_traj_raw, mpc_data_traj_raw] = - getData(mpc_traj_raw, current_steer, current_kinematics); + getData(m_mpc_traj_raw, current_steer, current_kinematics); if (!success_data_traj_raw) { return fail_warn_throttle("fail to get MPC Data for the raw trajectory. Stop MPC."); } @@ -192,11 +192,11 @@ void MPC::setReferenceTrajectory( const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position); - 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; @@ -233,7 +233,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 From b9b3e1b837a449a02f2ed11df5c92fbf377e2b27 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Mar 2024 05:29:16 +0000 Subject: [PATCH 19/23] style(pre-commit): autofix --- .../include/mpc_lateral_controller/mpc.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 aa965dce7a3dd..b2cb8a9cabbb1 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -407,7 +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. + 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. From d0cfc56e7a210e2590ea5c8864201cc5b0523be8 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 1 Mar 2024 15:52:46 +0900 Subject: [PATCH 20/23] set the default error output and update the readme Signed-off-by: Zhe Shen --- control/mpc_lateral_controller/README.md | 2 ++ .../include/mpc_lateral_controller/mpc.hpp | 3 +- control/mpc_lateral_controller/src/mpc.cpp | 36 ++++++++----------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 15f3d6741d201..2bb1a7c8a2d55 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -226,6 +226,8 @@ Some commonly used signals are listed below: | 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 b2cb8a9cabbb1..d956e5d8ce0e4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -380,7 +380,8 @@ class MPC 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) const; + const Odometry & current_kinematics, + bool success_data_for_diagnostic , bool success_data_traj_raw) const; /** * @brief calculate steering rate limit along with the target trajectory diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index ed7dcf1c14935..7731c8bf8649a 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -76,16 +76,10 @@ bool MPC::calculateMPC( // get the diagnostic data const auto [success_data_for_diagnostic, mpc_data_for_diagnostic] = getData(mpc_resampled_ref_trajectory, current_steer, current_kinematics); - if (!success_data_for_diagnostic) { - return fail_warn_throttle("fail to get MPC Data for the diagnostic. 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); - if (!success_data_traj_raw) { - return fail_warn_throttle("fail to get MPC Data for the raw trajectory. Stop MPC."); - } // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); @@ -125,7 +119,7 @@ bool MPC::calculateMPC( // prepare diagnostic message diagnostic = generateDiagData( mpc_resampled_ref_trajectory, mpc_data_traj_raw, mpc_data_for_diagnostic, mpc_matrix, ctrl_cmd, - Uex, current_kinematics); + Uex, current_kinematics, success_data_for_diagnostic, success_data_traj_raw); return true; } @@ -133,7 +127,7 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( 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) const + const VectorXd & Uex, const Odometry & current_kinematics, bool success_data_for_diagnostic , bool success_data_traj_raw) const { Float32MultiArrayStamped diagnostic; @@ -156,28 +150,28 @@ Float32MultiArrayStamped MPC::generateDiagData( append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) append_diag(Uex(0)); // [1] mpc calculation result 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(success_data_for_diagnostic ? std::atan(nearest_smooth_k * wb) : -1.0); // [3] feed-forward steering value raw + append_diag(success_data_for_diagnostic ? mpc_data.steer : -1.0); // [4] current steering angle + append_diag(success_data_for_diagnostic ? mpc_data.lateral_err : -1.0); // [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 - append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx)); // [9] reference velocity + append_diag(success_data_for_diagnostic ? tf2::getYaw(mpc_data.nearest_pose.orientation) : -1.0); // [7] nearest_pose yaw + append_diag(success_data_for_diagnostic ? mpc_data.yaw_err : -1.0); // [8] yaw error + append_diag(success_data_for_diagnostic ? reference_trajectory.vx.at(mpc_data.nearest_idx) : -1.0); // [9] reference velocity append_diag(current_velocity); // [10] measured velocity append_diag(wz_command); // [11] angular velocity from steer command - append_diag(wz_measured); // [12] angular velocity from measured steer - append_diag(current_velocity * nearest_smooth_k); // [13] angular velocity from path curvature - append_diag(nearest_smooth_k); // [14] nearest path curvature (used for feed-forward) - append_diag(nearest_k); // [15] nearest path curvature (not smoothed) - append_diag(mpc_data.predicted_steer); // [16] predicted steer - append_diag(wz_predicted); // [17] angular velocity from predicted steer + append_diag(success_data_for_diagnostic ? wz_measured : -1.0); // [12] angular velocity from measured steer + append_diag(success_data_for_diagnostic ? current_velocity * nearest_smooth_k : -1.0); // [13] angular velocity from path curvature + append_diag(success_data_for_diagnostic ? nearest_smooth_k : -1.0); // [14] nearest path curvature (used for feed-forward) + append_diag(success_data_for_diagnostic ? nearest_k : -1.0); // [15] nearest path curvature (not smoothed) + append_diag(success_data_for_diagnostic ? mpc_data.predicted_steer : -1.0); // [16] predicted steer + append_diag(success_data_for_diagnostic ? wz_predicted : -1.0); // [17] angular velocity from predicted steer 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(mpc_data_traj_raw.lateral_err); // [22] lateral error from raw trajectory + append_diag(success_data_traj_raw ? mpc_data_traj_raw.lateral_err : -1.0); // [22] lateral error from raw trajectory return diagnostic; } From f46c980d214a96d21bb18a80b17d94f33117a235 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Mar 2024 06:54:50 +0000 Subject: [PATCH 21/23] style(pre-commit): autofix --- .../include/mpc_lateral_controller/mpc.hpp | 4 +- control/mpc_lateral_controller/src/mpc.cpp | 58 +++++++++++++------ 2 files changed, 41 insertions(+), 21 deletions(-) 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 d956e5d8ce0e4..4d33491bf98c5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -380,8 +380,8 @@ class MPC 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_for_diagnostic , bool success_data_traj_raw) const; + const Odometry & current_kinematics, bool success_data_for_diagnostic, + bool success_data_traj_raw) const; /** * @brief calculate steering rate limit along with the target trajectory diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 7731c8bf8649a..5193082f5657b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -127,7 +127,8 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( 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_for_diagnostic , bool success_data_traj_raw) const + const VectorXd & Uex, const Odometry & current_kinematics, bool success_data_for_diagnostic, + bool success_data_traj_raw) const { Float32MultiArrayStamped diagnostic; @@ -147,31 +148,50 @@ Float32MultiArrayStamped MPC::generateDiagData( const auto append_diag = [&](const auto & val) -> void { diagnostic.data.push_back(static_cast(val)); }; - append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) - append_diag(Uex(0)); // [1] mpc calculation result - append_diag(mpc_matrix.Uref_ex(0)); // [2] feed-forward steering value - append_diag(success_data_for_diagnostic ? std::atan(nearest_smooth_k * wb) : -1.0); // [3] feed-forward steering value raw - append_diag(success_data_for_diagnostic ? mpc_data.steer : -1.0); // [4] current steering angle - append_diag(success_data_for_diagnostic ? mpc_data.lateral_err : -1.0); // [5] lateral error (the actual error used for MPC) + append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) + append_diag(Uex(0)); // [1] mpc calculation result + append_diag(mpc_matrix.Uref_ex(0)); // [2] feed-forward steering value + append_diag( + success_data_for_diagnostic ? std::atan(nearest_smooth_k * wb) + : -1.0); // [3] feed-forward steering value raw + append_diag(success_data_for_diagnostic ? mpc_data.steer : -1.0); // [4] current steering angle + append_diag( + success_data_for_diagnostic ? mpc_data.lateral_err + : -1.0); // [5] lateral error (the actual error used for MPC) append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation)); // [6] current_pose yaw - append_diag(success_data_for_diagnostic ? tf2::getYaw(mpc_data.nearest_pose.orientation) : -1.0); // [7] nearest_pose yaw - append_diag(success_data_for_diagnostic ? mpc_data.yaw_err : -1.0); // [8] yaw error - append_diag(success_data_for_diagnostic ? reference_trajectory.vx.at(mpc_data.nearest_idx) : -1.0); // [9] reference velocity - append_diag(current_velocity); // [10] measured velocity - append_diag(wz_command); // [11] angular velocity from steer command - append_diag(success_data_for_diagnostic ? wz_measured : -1.0); // [12] angular velocity from measured steer - append_diag(success_data_for_diagnostic ? current_velocity * nearest_smooth_k : -1.0); // [13] angular velocity from path curvature - append_diag(success_data_for_diagnostic ? nearest_smooth_k : -1.0); // [14] nearest path curvature (used for feed-forward) - append_diag(success_data_for_diagnostic ? nearest_k : -1.0); // [15] nearest path curvature (not smoothed) - append_diag(success_data_for_diagnostic ? mpc_data.predicted_steer : -1.0); // [16] predicted steer - append_diag(success_data_for_diagnostic ? wz_predicted : -1.0); // [17] angular velocity from predicted steer + append_diag( + success_data_for_diagnostic ? tf2::getYaw(mpc_data.nearest_pose.orientation) + : -1.0); // [7] nearest_pose yaw + append_diag(success_data_for_diagnostic ? mpc_data.yaw_err : -1.0); // [8] yaw error + append_diag( + success_data_for_diagnostic ? reference_trajectory.vx.at(mpc_data.nearest_idx) + : -1.0); // [9] reference velocity + append_diag(current_velocity); // [10] measured velocity + append_diag(wz_command); // [11] angular velocity from steer command + append_diag( + success_data_for_diagnostic ? wz_measured : -1.0); // [12] angular velocity from measured steer + append_diag( + success_data_for_diagnostic ? current_velocity * nearest_smooth_k + : -1.0); // [13] angular velocity from path curvature + append_diag( + success_data_for_diagnostic ? nearest_smooth_k + : -1.0); // [14] nearest path curvature (used for feed-forward) + append_diag( + success_data_for_diagnostic ? nearest_k : -1.0); // [15] nearest path curvature (not smoothed) + append_diag( + success_data_for_diagnostic ? mpc_data.predicted_steer : -1.0); // [16] predicted steer + append_diag( + success_data_for_diagnostic ? wz_predicted + : -1.0); // [17] angular velocity from predicted steer 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 + append_diag( + success_data_traj_raw ? mpc_data_traj_raw.lateral_err + : -1.0); // [22] lateral error from raw trajectory return diagnostic; } From dc98701f46db07d0a39892548285848d05f160e1 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 1 Mar 2024 16:36:23 +0900 Subject: [PATCH 22/23] remove the mpc_data_for_diag Signed-off-by: Zhe Shen --- .../include/mpc_lateral_controller/mpc.hpp | 2 +- control/mpc_lateral_controller/src/mpc.cpp | 51 ++++++------------- 2 files changed, 16 insertions(+), 37 deletions(-) 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 4d33491bf98c5..d906f14feb861 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -380,7 +380,7 @@ class MPC 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_for_diagnostic, + const Odometry & current_kinematics, bool success_data_traj_raw) const; /** diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 5193082f5657b..6dd14956d99d9 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -73,10 +73,6 @@ bool MPC::calculateMPC( return fail_warn_throttle("trajectory resampling failed. Stop MPC."); } - // get the diagnostic data - const auto [success_data_for_diagnostic, mpc_data_for_diagnostic] = - getData(mpc_resampled_ref_trajectory, current_steer, current_kinematics); - // 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); @@ -118,8 +114,8 @@ bool MPC::calculateMPC( // prepare diagnostic message diagnostic = generateDiagData( - mpc_resampled_ref_trajectory, mpc_data_traj_raw, mpc_data_for_diagnostic, mpc_matrix, ctrl_cmd, - Uex, current_kinematics, success_data_for_diagnostic, success_data_traj_raw); + reference_trajectory, mpc_data_traj_raw, mpc_data, mpc_matrix, ctrl_cmd, + Uex, current_kinematics, success_data_traj_raw); return true; } @@ -127,7 +123,7 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( 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_for_diagnostic, + const VectorXd & Uex, const Odometry & current_kinematics, bool success_data_traj_raw) const { Float32MultiArrayStamped diagnostic; @@ -151,38 +147,21 @@ Float32MultiArrayStamped MPC::generateDiagData( append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) append_diag(Uex(0)); // [1] mpc calculation result append_diag(mpc_matrix.Uref_ex(0)); // [2] feed-forward steering value - append_diag( - success_data_for_diagnostic ? std::atan(nearest_smooth_k * wb) - : -1.0); // [3] feed-forward steering value raw - append_diag(success_data_for_diagnostic ? mpc_data.steer : -1.0); // [4] current steering angle - append_diag( - success_data_for_diagnostic ? mpc_data.lateral_err - : -1.0); // [5] lateral error (the actual error used for MPC) + 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 (the actual error used for MPC) append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation)); // [6] current_pose yaw - append_diag( - success_data_for_diagnostic ? tf2::getYaw(mpc_data.nearest_pose.orientation) - : -1.0); // [7] nearest_pose yaw - append_diag(success_data_for_diagnostic ? mpc_data.yaw_err : -1.0); // [8] yaw error - append_diag( - success_data_for_diagnostic ? reference_trajectory.vx.at(mpc_data.nearest_idx) - : -1.0); // [9] reference velocity + append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation)); // [7] nearest_pose yaw + append_diag(mpc_data.yaw_err); // [8] yaw error + append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx)); // [9] reference velocity append_diag(current_velocity); // [10] measured velocity append_diag(wz_command); // [11] angular velocity from steer command - append_diag( - success_data_for_diagnostic ? wz_measured : -1.0); // [12] angular velocity from measured steer - append_diag( - success_data_for_diagnostic ? current_velocity * nearest_smooth_k - : -1.0); // [13] angular velocity from path curvature - append_diag( - success_data_for_diagnostic ? nearest_smooth_k - : -1.0); // [14] nearest path curvature (used for feed-forward) - append_diag( - success_data_for_diagnostic ? nearest_k : -1.0); // [15] nearest path curvature (not smoothed) - append_diag( - success_data_for_diagnostic ? mpc_data.predicted_steer : -1.0); // [16] predicted steer - append_diag( - success_data_for_diagnostic ? wz_predicted - : -1.0); // [17] angular velocity from predicted steer + append_diag(wz_measured); // [12] angular velocity from measured steer + append_diag(current_velocity * nearest_smooth_k); // [13] angular velocity from path curvature + append_diag(nearest_smooth_k); // [14] nearest path curvature (used for feed-forward) + append_diag(nearest_k); // [15] nearest path curvature (not smoothed) + append_diag(mpc_data.predicted_steer); // [16] predicted steer + append_diag(wz_predicted); // [17] angular velocity from predicted steer 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 From c7cef56f2726add56bab7c7fa1832307921e127f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Mar 2024 07:38:35 +0000 Subject: [PATCH 23/23] style(pre-commit): autofix --- .../include/mpc_lateral_controller/mpc.hpp | 3 +- control/mpc_lateral_controller/src/mpc.cpp | 33 +++++++++---------- 2 files changed, 17 insertions(+), 19 deletions(-) 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 d906f14feb861..87008701b4881 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -380,8 +380,7 @@ class MPC 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; + const Odometry & current_kinematics, bool success_data_traj_raw) const; /** * @brief calculate steering rate limit along with the target trajectory diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 6dd14956d99d9..4f0f82f80e093 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -114,8 +114,8 @@ bool MPC::calculateMPC( // prepare diagnostic message diagnostic = generateDiagData( - reference_trajectory, mpc_data_traj_raw, mpc_data, mpc_matrix, ctrl_cmd, - Uex, current_kinematics, success_data_traj_raw); + reference_trajectory, mpc_data_traj_raw, mpc_data, mpc_matrix, ctrl_cmd, Uex, + current_kinematics, success_data_traj_raw); return true; } @@ -123,8 +123,7 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( 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 + const VectorXd & Uex, const Odometry & current_kinematics, bool success_data_traj_raw) const { Float32MultiArrayStamped diagnostic; @@ -144,24 +143,24 @@ Float32MultiArrayStamped MPC::generateDiagData( const auto append_diag = [&](const auto & val) -> void { diagnostic.data.push_back(static_cast(val)); }; - append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) - append_diag(Uex(0)); // [1] mpc calculation result - append_diag(mpc_matrix.Uref_ex(0)); // [2] feed-forward steering value + append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) + append_diag(Uex(0)); // [1] mpc calculation result + 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.steer); // [4] current steering angle 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 - append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx)); // [9] reference velocity - append_diag(current_velocity); // [10] measured velocity - append_diag(wz_command); // [11] angular velocity from steer command - append_diag(wz_measured); // [12] angular velocity from measured steer + append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation)); // [7] nearest_pose yaw + append_diag(mpc_data.yaw_err); // [8] yaw error + append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx)); // [9] reference velocity + append_diag(current_velocity); // [10] measured velocity + append_diag(wz_command); // [11] angular velocity from steer command + append_diag(wz_measured); // [12] angular velocity from measured steer append_diag(current_velocity * nearest_smooth_k); // [13] angular velocity from path curvature - append_diag(nearest_smooth_k); // [14] nearest path curvature (used for feed-forward) - append_diag(nearest_k); // [15] nearest path curvature (not smoothed) + append_diag(nearest_smooth_k); // [14] nearest path curvature (used for feed-forward) + append_diag(nearest_k); // [15] nearest path curvature (not smoothed) append_diag(mpc_data.predicted_steer); // [16] predicted steer - append_diag(wz_predicted); // [17] angular velocity from predicted steer + append_diag(wz_predicted); // [17] angular velocity from predicted steer 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