nav2_waypoint_follower; Add an optional param to specify a sleep time…

[jediofgever]

Signed-off-by: jediofgever fetulahatas1@gmail.com

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(#1992)
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(Gazebo simulation of Turtlebot3)

---

Description of contribution in a few bullet points

• Added an additional/(optional) int parameter to nav2_waypoint_follower, in order to control behavior of robot in between waypoints. This int parameter species an amount of time in seconds to wait/sleep after reaching each waypoint. The parameter could be set to zero if a continuous execution(or as in original implementation) of waypoints is desired.

Description of documentation updates required from your changes

• Added new parameter, so need to add that to default configs and documentation page on doc

---

Future work that may be required in bullet points

• A more advanced concept for controlling robot's behavior on arrival of waypoints, could be based on service/action as mentioned by @SteveMacenski in Feature request within nav2_waypoint_follower #1992 (comment) , However I might open another ticker for that later on.

[jediofgever]

I targeted foxy-devel branch and did some very minor changes, but not sure why all checks are failing

[fmrico]

Hi, @jediofgever Did you passed the tests? I think that some code would not pass the linters...

[jediofgever]

Hi @fmrico , I haven't really run tests. Could you elaborate more on how we can run tests locally ?

[SteveMacenski]

@jediofgever you must target the main branch for any pull requests.

[SteveMacenski]

Another option I was thinking over this afternoon was adding a plugin for processing.

So rather than just hardcoding the sleep, we could create a really simple pluginlib interface for the user to load a plugin to do some behavior.

For example:

• Plugin has an initialize() and a run() function. Initialize will let the plugin setup any action / service / get parameters / etc for use.
• Then once we reach a waypoint, we give that waypoint pose to the run() function which can do whatever it likes. For this implementation, that plugin would just sleep for N seconds
• For a more specific use-case (like taking a picture or waiting for external input) then it could call a service, action, or wait for an event to occur before continuing on.

This would allow for all of those types of behaviors and abstract out the specifics to a run-time loadable plugin.

[jediofgever]

Applied requested changes and , changed trageted branch in this commit jediofgever@2a6d1b1

I opened a new PR #1996 with modified changes but CI builds are failing, interestingly on the package that I modified.
https://app.circleci.com/pipelines/github/ros-planning/navigation2/3798/workflows/32670729-f667-4f8c-8244-afe68822981a/jobs/14762

but from the log I see the error has appeared before I submitted #1996

I closed that #1996, I will open once the CI builds are back to normal.

[jediofgever]

Another option I was thinking over this afternoon was adding a plugin for processing.

So rather than just hardcoding the sleep, we could create a really simple pluginlib interface for the user to load a plugin to do some behavior.

processing this information ...
We agree that Each user's desired task to be executed on waypoint arrival could be different.
Therefore It is quite hard to think of a general purpose action server.

In my case, at each waypoint arrival, I had like call a action server which will notify user with QT pop-up, then depending on user's reaction, the robot will then proceed for the next waypoint.

I was thinking of adding a generic action such as ;

ExecuteTaskAtWaypoint.action

# Goal
string task_name
geometry_msgs/PoseStamped pose
---
# Result
builtin_interfaces/Duration total_elapsed_time
---
# Feedback
float32 percent_complete 

Then again the implementation of the server side for this generic action will be depending on desired behavior.

Going back to your comment;

So rather than just hardcoding the sleep, we could create a really simple pluginlib interface for the user to load a plugin to do some behavior.

I think that if it is decided that we add this ExecuteTaskAtWaypoint.action, this plugin will be basically a wrapper for the client side of purposed action, am I right ?

[SteveMacenski]

Please do not open new PRs for the same topic. Add your commit to this branch / PR for review. Submitting a new PR for each new commit makes it impossible to track changes and spams subscribers unnecessarily

that could work, but I think the plugin-based option is the most flexible. That way a user can make that "task" happen in the same process if they want, or call a remote server / action. Though having a defined message would make things easier for beginners since they wouldn't need to write any plugins.

What do you think? I'm still leaning towards plugins. I think that's the most flexible and makes no assumptions about the users' needs.

[fmrico]

Hi @fmrico , I haven't really run tests. Could you elaborate more on how we can run tests locally ?

Hi @jediofgever

To pass the tests, do a colcon test and check the log of the packages that didn't pass it. Tests include linters. In log dir, in latest_test, each package has a folder with stdout and stderr of this process. You can use the --packages-select option.

I checked in the CI details and I saw some style errors, like lines with spaces and { in the wrong line.

[jediofgever]

Please do not open new PRs for the same topic. Add your commit to this branch / PR for review. Submitting a new PR for each new commit makes it impossible to track changes and spams subscribers unnecessarily

my bad , I opened that PR because I targeted the wrong branch at first place, lets assume that PR never happened,I will add modified code into this PR.

Agreed that a run time loadable plugin will address this in a clean way. I can work towards that.
Should I add a very simple plugin into nav2_waypoint_follower, which will serve as a tutorial, and again it will be optional to add that into nav2_params.yaml
The plugin will basically do the same thing(if enabled) with above code, sleep for a defined time at waypoint arrival. Users can later create their own plugins for their specific needs, based on the template plugin.

Edit related to plugin;
I did some initial work towards having a plugin to do some tasks at waypoints.
Please note that this is only a draft and there likely will be modifications. However it will still be useful if you can have a look it and give me some feedback in order to improve lacking parts.
jediofgever@101ec93

[SteveMacenski]

Yes, a simple plugin definition in /include with implementations in /plugins would work. I think the API should be something like having an initialization function taking in a name and pointer to node. Then a processAtWaypoint() function taking in a pose & waypoint number to do whatever it likes (in this case, just sleeping). Maybe there's more info we can provide too, let me know if you think of something 😄

Actually, move the plugin header definition into nav2_core.

[jediofgever]

Applied the suggestions to the draft. I think the above commit is ready for a review. I hope to iterate through, in this week, I will take long break and might not be back quite soon.
In processAtWaypoint() we have provided all we can actually :)

void WaitAtWaypointArrival::processAtWaypoint(
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
  const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index);

in addition to that I guess I would be able to add the parameter entries to https://github.com/ros-planning/navigation.ros.org
soon.

[SteveMacenski]

Yes, for docs adding parameters to this repo's yaml and then the website's waypoint follower page. Additionally adding this to the website's migration guides & list in navigation plugins page. At some point (not part of this PR but hopefully if you're willing) a tutorial about how to create a custom plugin for their applications.

[SteveMacenski]

Really, really good first draft. I'm actually really impressed that you picked up on our plugin styling on your first PR in the project!

Just some stuff to clean up but overall I have no substantive issues.

Docs stuff: Yes, for docs adding parameters to this repo's yaml and then the website's waypoint follower page. Additionally adding this to the website's migration guides & adding to the list of navigation plugins. At some point (not part of this PR but hopefully if you're willing) a tutorial about how to create a custom plugin for their applications.

[jediofgever]

Thanks for the constructive feedback. I think I touched all the points on the requested changes. Please see above commit to make sure whether there is need for another iteration.

jediofgever/navigation.ros.org@35b1e6e is
an initial work towards adding documentation/configuration related to this plugin into website of navigation. Please find a time to roughly see if things looks at right place, or maybe I have missed some additional places that needed to be updated.

I am willing to add the tutorial to website as we have all needed code/classes.
However I had like to add that a while after this gets merged. Maybe for that tutorial we can aim taking pictures and saving them to a directory through waypoints, for inspection purposes or some other tasks, I am open to suggestions if there are any.

[SteveMacenski]

Just some smaller clean up

[SteveMacenski]

Oh here's a good idea, we should change this API so that processAtWaypoint can return a bool and we can check if its false to also treat it as a fail checking on stop_on_failure_ for whether to exit or continue to the next one

[jediofgever]

nice idea, I changed the return type of processAtWaypoint to bool, but if the plugin is disabled or waypoint_pause_duration is set to zero, we will always have to return false from processAtWaypoint call, right ? that will make stop_on_failure_ to be depending on the plugin to be enabled and active, not sure if that is behavior we want ?

[SteveMacenski]

If it fails to process & stop_on_failure_ is true, we should exit with failure.

I think maybe just not enabled returns true since its not a "failure", its just not turned on. The other option is that we make the default stopping time non-zero.

[jediofgever]

I will address this after we confirm whether there will be a follow up PR

[SteveMacenski]

Also this PR needs to target main not foxy-devel

On your docs branch:

• need to add to foxy -> galactic migration guide that this new plugin exists and why its there
• You have enabled and waypoint_pause_duration in the waypoitn follower API docs but those are in the specific plugin which is on another page. I think you need to remove it from the waypoint_follower page because its on the specific plugin page.
• On the plugins page, have the hyperlink go to the specific plugin implementation, not the pluginlib header

[jediofgever]

Also this PR needs to target main not foxy-devel

sure but main and foxy-devel seems to be diverged a lot, my branch is jediofgever:foxy-devel and if I target it to ros-planning:main wouldn't that introduce some unwanted things here ?

Followed your suggestions on docs branch and made another commit to address points. It will still need changes but priority now should be on how to merge this pull into ros-planning:main.

To me it seems that, I will need to add the latest changes in this pull(which you have already reviewed) to my fork (jediofgever:main), and open a new(unfortunately) pull request which targets to ros-planning:main
is there another way around, what do you think ?

[SteveMacenski]

I don't know, but we do not accept new feature development to a targeted distribution, it must be in main. You may have some conflicts to resolve but I doubt it, you're messing with code that hasn't changed in a long time.

We also need that docs PR to be submitted as well before this can be merged. I want to make sure that we keep things up to date.

[SteveMacenski]

@jediofgever any update? That last thing with stop on failure is the last thing (other than going to main) for merging this!

[jediofgever]

sorry I was away for a local holiday. Will update here soon.

[SteveMacenski]

No worries, enjoy your vacation 🌴

[naiveHobo]

As much as I like the concept this PR introduces, isn't the whole idea of executing a behavior after reaching a waypoint better suited to be executed as part of the behavior tree? A very simple way of sleeping after reaching a waypoint with the current architecture would be by adding a Wait BT action node in sequence with the existing default behavior tree. A more nav2 way of solving it would be to implement waypoint follower itself as a BT (#1928) and adding complex behaviors to this BT. Thoughts?

[jediofgever]

@SteveMacenski, I think it is resolved here ? waypoint_task_executor==> WaitAtWaypoint

[jediofgever]

It seems hard to elegantly change target branch to main with current status,
there is a lot of conflicting files, It was my mistake to target foxy-devel at first place, let me fix that by placing latest changes to main and submit a follow up PR. This will make things look easier to understand and cleaner.

Edit;
I have latest changes on top of main now. I will wait a confirmation whether to submit a follow-up PR.

relevant PR for navigation.ros.org has been submitted, ros-navigation/docs.nav2.org#91

==========================================================================================
--------------------------------------------------------------------------------------------------------------------------------------------------------------==========================================================================================

@naiveHobo ,

isn't the whole idea of executing a behavior after reaching a waypoint better suited to be executed as part of the behavior tree?

It well could be, but to my understanding implementing the concept of this PR with BT could be an overkill(or misuse of BT). The rationale behind this for me is that; I have hard time to see multi-step or many state characteristic of aimed feature(Executing a task on arrival of waypoint). However I would be curious about additional insights from people following this PR. As for implementing waypoint follower itself with BT, I would observe what @SteveMacenski and other original authors says.

[codecov]

Codecov Report

Merging #1993 into main will increase coverage by 15.33%.
The diff coverage is 90.16%.

@@             Coverage Diff             @@
##             main    #1993       +/-   ##
===========================================
+ Coverage   69.39%   84.73%   +15.33%     
===========================================
  Files         212      294       +82     
  Lines       10595    15056     +4461     
===========================================
+ Hits         7352    12757     +5405     
+ Misses       3243     2299      -944     

Flag	Coverage Δ
#project	84.73% <90.16%> (+15.33%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...est/plugins/condition/test_transform_available.cpp	100.00% <ø> (ø)
..._behavior_tree/test/test_behavior_tree_fixture.hpp	100.00% <ø> (ø)
...roller/include/nav2_controller/nav2_controller.hpp	100.00% <ø> (ø)
nav2_core/include/nav2_core/controller.hpp	100.00% <ø> (ø)
nav2_core/include/nav2_core/global_planner.hpp	100.00% <ø> (ø)
nav2_core/include/nav2_core/goal_checker.hpp	100.00% <ø> (ø)
nav2_core/include/nav2_core/recovery.hpp	100.00% <ø> (ø)
..._costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp	100.00% <ø> (ø)
...d/include/nav2_costmap_2d/costmap_2d_publisher.hpp	100.00% <ø> (ø)
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	73.33% <0.00%> (+9.69%)	⬆️
... and 256 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 7f8c7b6...3369ca6. Read the comment docs.

[SteveMacenski]

isn't the whole idea of executing a behavior after reaching a waypoint better suited to be executed as part of the behavior tree

That's one option, but sometimes that's overkill. Sometimes you just need a robot to follow some points and do something at each point, and in that case this could be a more streamline method. Plus if we wanted to add custom behavior to the WP follower this would be required, so I like the idea of making it a framework vs patching for every single possible thing someone might want to do at a waypoint for testing.

I look at this and say "better is good". As long as we're wanting to have a basic WP follower node, we should make it the best WP follower node it can be.

@jediofgever this PR has become somewhat of a mess, there's a ton of stuff in here that should not be in here from incorrect rebasing or conflicting branches. I'm not sure what you did, but please revert those changes. If you need to cherry pick your commits over to the main branch and reopen a new branch PR, that's fine too.

[jediofgever]

this PR has become somewhat of a mess

Agreed, but no worries I will handle it