feat(autoware_velocity_smoother): add debug msg

Signed-off-by: TetsuKawa <[email protected]>
tier4 · Dec 6, 2024 · 9f74867 · 9f74867
1 parent 0d15538
commit 9f74867
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp
@@ -353,6 +353,8 @@ void VelocitySmootherNode::calcExternalVelocityLimit()
 
   // sender
   external_velocity_limit_.sender = external_velocity_limit_ptr_->sender;
+  RCLCPP_INFO(
+    get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());
 
   // on the first time, apply directly
   if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
@@ -392,6 +394,8 @@ void VelocitySmootherNode::calcExternalVelocityLimit()
         external_velocity_limit_ptr_->use_constraints ? cstr.max_jerk : smoother_->getMaxJerk();
       const auto j_min =
         external_velocity_limit_ptr_->use_constraints ? cstr.min_jerk : smoother_->getMinJerk();
+      RCLCPP_INFO(
+        get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);
 
       // If the closest acceleration is positive, velocity will increase
       // until the acceleration becomes zero
@@ -401,6 +405,8 @@ void VelocitySmootherNode::calcExternalVelocityLimit()
       } else {
         max_velocity_with_deceleration_ = v0;
       }
+      RCLCPP_INFO(
+        get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);
 
       if (external_velocity_limit_ptr_->max_velocity < max_velocity_with_deceleration_) {
         // TODO(mkuri) If v0 < external_velocity_limit_ptr_->max_velocity <
@@ -421,9 +427,13 @@ void VelocitySmootherNode::calcExternalVelocityLimit()
           RCLCPP_WARN(get_logger(), "Stop distance calculation failed!");
         }
         external_velocity_limit_.dist = stop_dist + margin;
+        RCLCPP_INFO(
+          get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
       } else {
         max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity;
         external_velocity_limit_.dist = 0.0;
+        RCLCPP_INFO(
+          get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
       }
     }
   }
@@ -958,6 +968,9 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c
   // apply external velocity limit from the inserted point
   trajectory_utils::applyMaximumVelocityLimit(
     *inserted_index, traj.size(), external_velocity_limit_.velocity, traj);
+  RCLCPP_INFO(
+    get_logger(), "apply external velocity limit: dist = %f, vel = %f",
+    external_velocity_limit_.dist, external_velocity_limit_.velocity);
 
   // create virtual wall
   if (std::abs(external_velocity_limit_.velocity) < 1e-3) {