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

Support starting on the last pose of planned path instead of the exact waypoint in compute path through poses action #3928

Closed
wants to merge 8 commits into from
Closed

Support starting on the last pose of planned path instead of the exact waypoint in compute path through poses action #3928

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
be2904e
Support stitching paths in compute path to poses
pepisg Nov 2, 2023
a939eab
Update nav2_planner/src/planner_server.cpp
pepisg Nov 2, 2023
f2efccd
Rename parameter to allow_path_through_poses_goal_deviation
pepisg Dec 10, 2023
92a443d
Merge branch 'allow_path_end_pose_deviation' of github.com:kiwicampus…
pepisg Dec 10, 2023
0b64122
Fix description
pepisg Dec 10, 2023
6b1a8a2
Merge branch 'main' of github.com:kiwicampus/navigation2 into allow_p…
pepisg Dec 10, 2023
24b7a57
restore nav2_params
pepisg Dec 10, 2023
409ead1
missing whitespace
pepisg Dec 10, 2023
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 nav2_planner/include/nav2_planner/planner_server.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -240,6 +240,7 @@ class PlannerServer : public nav2_util::LifecycleNode
std::vector<std::string> planner_types_;
double max_planner_duration_;
std::string planner_ids_concat_;
bool allow_path_through_poses_goal_deviation_;

// TF buffer
std::shared_ptr<tf2_ros::Buffer> tf_;
Expand Down
16 changes: 15 additions & 1 deletion nav2_planner/src/planner_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
// Declare this node's parameters
declare_parameter("planner_plugins", default_ids_);
declare_parameter("expected_planner_frequency", 1.0);
declare_parameter("allow_path_through_poses_goal_deviation", true);

declare_parameter("action_server_result_timeout", 10.0);

Expand Down Expand Up @@ -137,6 +138,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
" than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency);
max_planner_duration_ = 0.0;
}
get_parameter("allow_path_through_poses_goal_deviation", allow_path_through_poses_goal_deviation_);

// Initialize pubs & subs
plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
Expand Down Expand Up @@ -395,7 +397,15 @@ void PlannerServer::computePlanThroughPoses()
if (i == 0) {
curr_start = start;
} else {
curr_start = goal->goals[i - 1];
if (allow_path_through_poses_goal_deviation_) {
// pick the end of the last planning task as the start for the next one
pepisg marked this conversation as resolved.
Show resolved Hide resolved
// to allow for path tolerance deviations
curr_start = concat_path.poses.back();
Copy link
Member

@SteveMacenski SteveMacenski Nov 2, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Curious: test with Smac Hybrid, Lattice, NavFn. Does the curr_start == goal->goals[i - 1] or within floating point error of it? I believe it might. If so, we don't even necessarily need to make this a parameter nor a if/else condition. We could just do this as the default behavior and add to our migration guide that it supports deviation up to the planner's tolerance.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tested with this planners and it seems to work (hybrid and lattice seem to be harder to use in close proximity of obstacles in general with the default configuration though):
NavFn:
Screenshot from 2023-11-06 07-56-09
Hybrid:
Screenshot from 2023-11-06 08-09-23
Lattice:
Screenshot from 2023-11-06 08-30-42

We could just do this as the default behavior and add to our migration guide that it supports deviation up to the planner's tolerance.

I agree this should be the default behavior: if there's a user defined tolerance navigation should succeed as long as the planner can get within that tolerance to goals, I can't think of a good example where an user may want compute_path_though_poses to fail where that is possible. They can always set a very low tolerance if having goals in non-traversable space is a critical condition

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Lattice shows a strange discontinuity - its hard for me to tell from the resolution of the picture of that's a problem or not. Is that within a single costmap cell (and just goal approximation within a cell issues)? Does that happen when the approximate option is disabled?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pepisg any update?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hey! I havent had time to keep reviewing this, specially for writing the new tests for the navigate through poses action (planner tests seem to test the functions on planner server directly, so it looked like a complete new testing module to test the actions would have to be written).

I will try to make time for this in the coming weeks, sorry for the delay

curr_start.header = concat_path.header;
} else {
// pick the exact pose task as the start for the next one
curr_start = goal->goals[i - 1];
}
}
curr_goal = goal->goals[i];

Expand Down Expand Up @@ -680,6 +690,10 @@ PlannerServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
max_planner_duration_ = 0.0;
}
}
} else if (type == ParameterType::PARAMETER_BOOL) {
if (name == "allow_path_through_poses_goal_deviation") {
allow_path_through_poses_goal_deviation_ = parameter.as_bool();
}
}
}

Expand Down