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

fix(freespace_planner): fix motion_velocity_smoother error while parking #6713

Merged
merged 2 commits into from
May 9, 2024
Merged

fix(freespace_planner): fix motion_velocity_smoother error while parking #6713

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
e7f0199
fix(freespace_planner): prevent publishing stop trajectory with only …
ahmeddesokyebrahim Mar 29, 2024
01bfb5a
fix(freespace_planner): mark flag with true when trajectory is less t…
ahmeddesokyebrahim Mar 29, 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
1 change: 1 addition & 0 deletions planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,7 @@ class FreespacePlannerNode : public rclcpp::Node
size_t target_index_;
bool is_completed_ = false;
bool reset_in_progress_ = false;
bool is_new_parking_cycle_ = true;

LaneletRoute::ConstSharedPtr route_;
OccupancyGrid::ConstSharedPtr occupancy_grid_;
Expand Down
22 changes: 15 additions & 7 deletions planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -321,6 +321,8 @@
goal_pose_.header = msg->header;
goal_pose_.pose = msg->goal_pose;

is_new_parking_cycle_ = true;

reset();
}

Expand Down Expand Up @@ -447,46 +449,52 @@
// Must stop before replanning any new trajectory
const bool is_reset_required = !reset_in_progress_ && isPlanRequired();
if (is_reset_required) {
// Stop before planning new trajectory
const auto stop_trajectory = partial_trajectory_.points.empty()
? createStopTrajectory(current_pose_)
: createStopTrajectory(partial_trajectory_);
trajectory_pub_->publish(stop_trajectory);
debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
// Stop before planning new trajectory, except in a new parking cycle as the vehicle already
// stops.
if (!is_new_parking_cycle_) {
const auto stop_trajectory = partial_trajectory_.points.empty()
? createStopTrajectory(current_pose_)
: createStopTrajectory(partial_trajectory_);
trajectory_pub_->publish(stop_trajectory);
debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
}

reset();

reset_in_progress_ = true;
}

if (reset_in_progress_) {
const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps);
if (is_stopped) {
// Plan new trajectory
planTrajectory();
reset_in_progress_ = false;
} else {
// Will keep current stop trajectory
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 1000,
"Waiting for the vehicle to stop before generating a new trajectory.");
}
}

// StopTrajectory
if (trajectory_.points.size() <= 1) {
is_new_parking_cycle_ = false;
return;
}

// Update partial trajectory
updateTargetIndex();
partial_trajectory_ = getPartialTrajectory(trajectory_, prev_target_index_, target_index_);

// Publish messages
trajectory_pub_->publish(partial_trajectory_);
debug_pose_array_pub_->publish(trajectory2PoseArray(trajectory_));
debug_partial_pose_array_pub_->publish(trajectory2PoseArray(partial_trajectory_));

is_new_parking_cycle_ = false;

Check notice on line 497 in planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Complex Method 

FreespacePlannerNode::onTimer increases in cyclomatic complexity from 14 to 15, 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 warning on line 497 in planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

FreespacePlannerNode::onTimer has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested 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.
}

void FreespacePlannerNode::planTrajectory()
Expand Down
Loading