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

Jump to bottom

perf(velocity_smoother): not resample debug_trajectories is not used #8030

Merged
merged 2 commits into from
Jul 30, 2024
Merged

perf(velocity_smoother): not resample debug_trajectories is not used #8030

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
246b625
not resample debug_trajectories if not published
takayuki5168 Jul 12, 2024
c644ec3
update dependant packages
takayuki5168 Jul 30, 2024
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
3 changes: 2 additions & 1 deletion ...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
const bool publish_debug_trajs) override;

TrajectoryPoints resampleTrajectory(
const TrajectoryPoints & input, [[maybe_unused]] const double v0,
Expand Down
3 changes: 2 additions & 1 deletion ..._velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
const bool publish_debug_trajs) override;

TrajectoryPoints resampleTrajectory(
const TrajectoryPoints & input, [[maybe_unused]] const double v0,
Expand Down
3 changes: 2 additions & 1 deletion ...velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
const bool publish_debug_trajs) override;

TrajectoryPoints resampleTrajectory(
const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
Expand Down
3 changes: 2 additions & 1 deletion ...locity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
const bool publish_debug_trajs) override;

TrajectoryPoints resampleTrajectory(
const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
Expand Down
3 changes: 2 additions & 1 deletion .../autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,8 @@ class SmootherBase
virtual ~SmootherBase() = default;
virtual bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) = 0;
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
const bool publish_debug_trajs) = 0;

virtual TrajectoryPoints resampleTrajectory(
const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
Expand Down
19 changes: 9 additions & 10 deletions planning/autoware_velocity_smoother/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/autoware_velocity_smoother/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.26 to 4.23, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -629,55 +629,54 @@

std::vector<TrajectoryPoints> debug_trajectories;
if (!smoother_->apply(
initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) {
initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories,
publish_debug_trajs_)) {
RCLCPP_WARN(get_logger(), "Fail to solve optimization.");
}

// Set 0 velocity after input-stop-point
overwriteStopPoint(clipped, traj_smoothed);

traj_smoothed.insert(
traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);

// For the endpoint of the trajectory
if (!traj_smoothed.empty()) {
traj_smoothed.back().longitudinal_velocity_mps = 0.0;
}

// Max velocity filter for safety
trajectory_utils::applyMaximumVelocityLimit(
traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed);

// Insert behind velocity for output's consistency
insertBehindVelocity(traj_resampled_closest, type, traj_smoothed);

RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size());
if (publish_debug_trajs_) {
{
auto tmp = traj_lateral_acc_filtered;
if (is_reverse_) flipVelocity(tmp);
pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp));
}
{
auto tmp = traj_resampled;
if (is_reverse_) flipVelocity(tmp);
pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
}
{
auto tmp = traj_steering_rate_limited;
if (is_reverse_) flipVelocity(tmp);
pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp));
}

if (!debug_trajectories.empty()) {
for (auto & debug_trajectory : debug_trajectories) {
debug_trajectory.insert(
debug_trajectory.begin(), traj_resampled.begin(),
traj_resampled.begin() + traj_resampled_closest);
for (size_t i = 0; i < traj_resampled_closest; ++i) {
debug_trajectory.at(i).longitudinal_velocity_mps =
debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps;
}
for (auto & debug_trajectory : debug_trajectories) {
debug_trajectory.insert(
debug_trajectory.begin(), traj_resampled.begin(),
traj_resampled.begin() + traj_resampled_closest);
for (size_t i = 0; i < traj_resampled_closest; ++i) {
debug_trajectory.at(i).longitudinal_velocity_mps =

Check warning on line 678 in planning/autoware_velocity_smoother/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_velocity_smoother/src/node.cpp#L678 

Added line #L678 was not covered by tests
debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps;

Check notice on line 679 in planning/autoware_velocity_smoother/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

VelocitySmootherNode::smoothVelocity decreases in cyclomatic complexity from 14 to 13, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 679 in planning/autoware_velocity_smoother/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Deep, Nested Complexity 

VelocitySmootherNode::smoothVelocity is no longer above the threshold for nested complexity depth. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
}
}
publishDebugTrajectories(debug_trajectories);
Expand Down
3 changes: 2 additions & 1 deletion ...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,8 @@

bool AnalyticalJerkConstrainedSmoother::apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories)
TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories,
[[maybe_unused]] const bool publish_debug_trajs)

Check warning on line 107 in planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

AnalyticalJerkConstrainedSmoother::apply already has high cyclomatic complexity, and now it increases in Lines of Code from 116 to 117. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
{
RCLCPP_DEBUG(logger_, "-------------------- Start --------------------");

Expand Down
21 changes: 13 additions & 8 deletions planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 6.00 to 6.25, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -59,79 +59,84 @@

bool JerkFilteredSmoother::apply(
const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output,
std::vector<TrajectoryPoints> & debug_trajectories)
std::vector<TrajectoryPoints> & debug_trajectories, const bool publish_debug_trajs)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

output = input;

if (input.empty()) {
RCLCPP_WARN(logger_, "Input TrajectoryPoints to the jerk filtered optimization is empty.");
return false;
}

if (input.size() == 1) {
// No need to do optimization
output.front().longitudinal_velocity_mps = v0;
output.front().acceleration_mps2 = a0;
debug_trajectories.resize(3);
debug_trajectories[0] = output;
debug_trajectories[1] = output;
debug_trajectories[2] = output;
return true;
}

const auto ts = std::chrono::system_clock::now();

const double a_max = base_param_.max_accel;
const double a_min = base_param_.min_decel;
const double a_stop_accel = 0.0;
const double a_stop_decel = base_param_.stop_decel;
const double j_max = base_param_.max_jerk;
const double j_min = base_param_.min_jerk;
const double over_j_weight = smoother_param_.over_j_weight;
const double over_v_weight = smoother_param_.over_v_weight;
const double over_a_weight = smoother_param_.over_a_weight;

// jerk filter
const auto forward_filtered =
forwardJerkFilter(v0, std::max(a0, a_min), a_max, a_stop_accel, j_max, input);
const auto backward_filtered = backwardJerkFilter(
input.back().longitudinal_velocity_mps, a_stop_decel, a_min, a_stop_decel, j_min, input);
const auto filtered =
mergeFilteredTrajectory(v0, a0, a_min, j_min, forward_filtered, backward_filtered);

// Resample TrajectoryPoints for Optimization
// TODO(planning/control team) deal with overlapped lanes with the same direction
const auto initial_traj_pose = filtered.front().pose;

const auto resample = [&](const auto & trajectory) {
autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_);

return resampling::resampleTrajectory(
trajectory, v0, initial_traj_pose, std::numeric_limits<double>::max(),
std::numeric_limits<double>::max(), base_param_.resample_param);
};

auto opt_resampled_trajectory = resample(filtered);

// Set debug trajectories
debug_trajectories.resize(3);
debug_trajectories[0] = resample(forward_filtered);
debug_trajectories[1] = resample(backward_filtered);
debug_trajectories[2] = resample(filtered);
if (publish_debug_trajs) {
debug_trajectories.resize(3);
debug_trajectories[0] = resample(forward_filtered);
debug_trajectories[1] = resample(backward_filtered);
debug_trajectories[2] = resample(filtered);
}

// Ensure terminal velocity is zero
opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0;

// If Resampled Size is too small, we don't do optimization
if (opt_resampled_trajectory.size() == 1) {
// No need to do optimization
output.front().longitudinal_velocity_mps = v0;
output.front().acceleration_mps2 = a0;
debug_trajectories[0] = output;
debug_trajectories[1] = output;
debug_trajectories[2] = output;
if (publish_debug_trajs) {
debug_trajectories.resize(3);
debug_trajectories[0] = output;
debug_trajectories[1] = output;
debug_trajectories[2] = output;
}

Check warning on line 139 in planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

JerkFilteredSmoother::apply increases in cyclomatic complexity from 26 to 28, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 139 in planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead 

JerkFilteredSmoother::apply increases from 3 to 4 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check notice on line 139 in planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Excess Number of Function Arguments 

JerkFilteredSmoother::apply increases from 5 to 6 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
return true;
}

Expand Down
3 changes: 2 additions & 1 deletion planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@

bool L2PseudoJerkSmoother::apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
[[maybe_unused]] const bool publish_debug_trajs)

Check warning on line 57 in planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

L2PseudoJerkSmoother::apply already has high cyclomatic complexity, and now it increases in Lines of Code from 123 to 124. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 57 in planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

L2PseudoJerkSmoother::apply is no longer above the threshold for number of arguments
{
debug_trajectories.clear();

Expand Down
3 changes: 2 additions & 1 deletion planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@

bool LinfPseudoJerkSmoother::apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)
TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
[[maybe_unused]] const bool publish_debug_trajs)

Check warning on line 57 in planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

LinfPseudoJerkSmoother::apply already has high cyclomatic complexity, and now it increases in Lines of Code from 133 to 134. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 57 in planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

LinfPseudoJerkSmoother::apply is no longer above the threshold for number of arguments
{
debug_trajectories.clear();

Expand Down
2 changes: 1 addition & 1 deletion ...ty_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ bool smoothPath(
TrajectoryPoints traj_smoothed;
clipped.insert(
clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) {
std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
return false;
}
Expand Down
2 changes: 1 addition & 1 deletion planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -361,7 +361,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed;
clipped.insert(
clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) {
RCLCPP_ERROR(get_logger(), "failed to smooth");
}
traj_smoothed.insert(
Expand Down
Loading