From 8092cdbb1e636d1492506eb9bb5dfab5a3f7ba8c Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 6 Dec 2024 21:04:31 +0900 Subject: [PATCH] feat: add debug msg Signed-off-by: TetsuKawa --- planning/autoware_velocity_smoother/src/node.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index a8a3a847adec3..25aab78cf1a98 100644 --- 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) {