feat: enable debug publishing of predicted and resampled reference tr…

…ajectories This commit enables the debug publishing of predicted and resampled reference trajectories for debugging purposes. It adds a new flag, `publish_debug_trajectories`, which is set to `true` by default. Signed-off-by: Kyoichi Sugahara <[email protected]>
autowarefoundation · Sep 2, 2024 · 163ed7d · 163ed7d
1 parent 0eea192
commit 163ed7d
Show file tree

Hide file tree

Showing 6 changed files with 10 additions and 15 deletions.
diff --git a/control/autoware_mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md
@@ -205,10 +205,9 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as
 
 #### Debug
 
-| Name                                         | Type    | Description                                                                      | Default value |
-| :------------------------------------------- | :------ | :------------------------------------------------------------------------------- | :------------ |
-| debug_publish_predicted_trajectory           | boolean | publish predicted trajectory for debugging, considering Frenet coordinate system | true          |
-| debug_publish_resampled_reference_trajectory | boolean | publish resampled reference trajectory in world coordinate system                | false         |
+| Name                       | Type    | Description                                                                       | Default value |
+| :------------------------- | :------ | :-------------------------------------------------------------------------------- | :------------ |
+| publish_debug_trajectories | boolean | publish predicted trajectory and resampled reference trajectory for debug purpose | true          |
 
 ### How to tune MPC parameters
 

diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp
@@ -433,9 +433,8 @@ class MPC
   bool m_use_delayed_initial_state =
     true;  // Flag to use x0_delayed as initial state for predicted trajectory
 
-  bool m_debug_publish_predicted_trajectory = false;  // Flag to publish debug predicted trajectory
-  bool m_debug_publish_resampled_reference_trajectory = false;  // Flag to publish debug resampled
-                                                                // reference trajectory
+  bool m_publish_debug_trajectories = false;  // Flag to publish predicted trajectory and
+                                              // resampled reference trajectory for debug purpose
 
   //!< Constructor.
   explicit MPC(rclcpp::Node & node);

diff --git a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml
@@ -85,5 +85,4 @@
     cf: 155494.663
     cr: 155494.663
 
-    debug_publish_predicted_trajectory: true  # publish debug predicted trajectory in Frenet coordinate
-    debug_publish_resampled_reference_trajectory: false  # publish debug resampled reference trajectory
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose
diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp
@@ -111,7 +111,7 @@ bool MPC::calculateMPC(
     mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "world");
 
   // Publish predicted trajectories in different coordinates for debugging purposes
-  if (m_debug_publish_predicted_trajectory) {
+  if (m_publish_debug_trajectories) {
     // Calculate and publish predicted trajectory in Frenet coordinate
     auto predicted_trajectory_frenet = calculatePredictedTrajectory(
       mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "frenet");
@@ -324,7 +324,7 @@ std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
     return {false, {}};
   }
   // Publish resampled reference trajectory for debug purpose.
-  if (m_debug_publish_resampled_reference_trajectory) {
+  if (m_publish_debug_trajectories) {
     auto converted_output = MPCUtils::convertToAutowareTrajectory(output);
     converted_output.header.stamp = m_clock->now();
     converted_output.header.frame_id = "map";

diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp
@@ -140,8 +140,7 @@ MpcLateralController::MpcLateralController(
   m_mpc->m_use_delayed_initial_state = dp_bool("use_delayed_initial_state");
 
   m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory");
-  m_mpc->m_debug_publish_resampled_reference_trajectory =
-    dp_bool("debug_publish_resampled_reference_trajectory");
+  m_mpc->m_publish_debug_trajectories = dp_bool("publish_debug_trajectories");
 
   m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
   m_pub_debug_values =

diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml
@@ -76,5 +76,4 @@
       average_num: 1000
       steering_offset_limit: 0.02
 
-    debug_publish_predicted_trajectory: true  # publish debug predicted trajectory in Frenet coordinate
-    debug_publish_resampled_reference_trajectory: false  # publish debug resampled reference trajectory
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose