debug(motion_velocity_smoother): add debug messages for external

velocity limit

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -342,6 +342,7 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
   if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
     external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity;
     pub_velocity_limit_->publish(*external_velocity_limit_ptr_);
+    RCLCPP_INFO(get_logger(), "on the first time, apply directly");
     return;
   }
 
@@ -364,6 +365,7 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
     eps) {
     const double v0 = current_closest_point_from_prev_output_->longitudinal_velocity_mps;
     const double a0 = current_closest_point_from_prev_output_->acceleration_mps2;
+    RCLCPP_INFO(get_logger(), "v0 = %f, a0 = %f", v0, a0);
 
     if (isEngageStatus(v0)) {
       max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity;
@@ -376,6 +378,7 @@ void MotionVelocitySmootherNode::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
@@ -385,6 +388,7 @@ void MotionVelocitySmootherNode::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 <
@@ -405,9 +409,11 @@ void MotionVelocitySmootherNode::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);
       }
     }
   }
@@ -527,7 +533,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit()
   // calculate distance to insert external velocity limit
   const double travel_dist = calcTravelDistance();
   external_velocity_limit_.dist = std::max(external_velocity_limit_.dist - travel_dist, 0.0);
-  RCLCPP_DEBUG(
+  RCLCPP_INFO(
     get_logger(), "run: travel_dist = %f, external_velocity_limit_dist_ = %f", travel_dist,
     external_velocity_limit_.dist);
 
@@ -904,6 +910,8 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t
   // 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) {