Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_velocity_smoother): add debug msg #1679

Merged
merged 1 commit into from
Dec 6, 2024
Merged

feat(autoware_velocity_smoother): add debug msg #1679

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions planning/autoware_velocity_smoother/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_) {
Expand Down Expand Up @@ -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
Expand All @@ -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 <
Expand All @@ -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);
}
}
}
Expand Down Expand Up @@ -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) {
Expand Down
Loading