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(freespace_planner): only plan if ego is stopped #6062

Merged
merged 5 commits into from
May 8, 2024
Merged

feat(freespace_planner): only plan if ego is stopped #6062

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
510317e
feat(freespace_planner): only plan if ego is stopped
VRichardJP Jan 12, 2024
17f5674
style(pre-commit): autofix
pre-commit-ci[bot] Jan 12, 2024
81790e1
Update planning/freespace_planner/src/freespace_planner/freespace_pla…
VRichardJP Feb 8, 2024
03c5aca
Merge branch 'main' into freespace_planner_wait_for_stopped
maxime-clem Mar 8, 2024
648fce9
Merge branch 'main' into freespace_planner_wait_for_stopped
maxime-clem May 8, 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 @@ -133,6 +133,7 @@ class FreespacePlannerNode : public rclcpp::Node
size_t prev_target_index_;
size_t target_index_;
bool is_completed_ = false;
bool reset_in_progress_ = false;

LaneletRoute::ConstSharedPtr route_;
OccupancyGrid::ConstSharedPtr occupancy_grid_;
Expand Down
21 changes: 18 additions & 3 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 @@ -441,7 +441,9 @@ void FreespacePlannerNode::onTimer()
return;
}

if (isPlanRequired()) {
// Must stop before replanning any new trajectory
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_)
Expand All @@ -452,8 +454,21 @@ void FreespacePlannerNode::onTimer()

reset();

// Plan new trajectory
planTrajectory();
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
Expand Down