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.

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

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

Closed
wants to merge 6 commits into from
Closed

Conversation

jediofgever
Copy link
Contributor

@jediofgever jediofgever commented Sep 23, 2020

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

… inbetween waypoints.

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
@jediofgever
Copy link
Contributor Author

jediofgever commented Sep 23, 2020

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

@fmrico
Copy link
Contributor

fmrico commented Sep 23, 2020

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

@jediofgever
Copy link
Contributor Author

jediofgever commented Sep 23, 2020

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

@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 23, 2020

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

Copy link
Member

@SteveMacenski SteveMacenski left a comment

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.

doc/parameters/param_list.md Outdated Show resolved Hide resolved
nav2_bringup/bringup/params/nav2_params.yaml Outdated Show resolved Hide resolved
nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved
nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved
nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved
@jediofgever
Copy link
Contributor Author

jediofgever commented Sep 24, 2020

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
Copy link
Contributor Author

jediofgever commented Sep 24, 2020

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
Copy link
Member

SteveMacenski commented Sep 24, 2020

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
Copy link
Contributor

fmrico commented Sep 24, 2020

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
Copy link
Contributor Author

jediofgever commented Sep 25, 2020

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

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
doc/parameters/param_list.md Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 25, 2020

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
Copy link
Contributor Author

jediofgever commented Sep 28, 2020

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
Copy link
Member

SteveMacenski commented Sep 28, 2020

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.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

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.

doc/parameters/param_list.md Outdated Show resolved Hide resolved
doc/parameters/param_list.md Outdated Show resolved Hide resolved
nav2_bringup/bringup/params/nav2_params.yaml Outdated Show resolved Hide resolved
nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved
nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved
@jediofgever
Copy link
Contributor Author

jediofgever commented Sep 29, 2020

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.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Just some smaller clean up

doc/parameters/param_list.md Outdated Show resolved Hide resolved
nav2_waypoint_follower/plugins/wait_at_waypoint.cpp Outdated Show resolved Hide resolved
nav2_waypoint_follower/plugins/wait_at_waypoint.cpp Outdated Show resolved Hide resolved
nav2_waypoint_follower/plugins/wait_at_waypoint.cpp Outdated Show resolved Hide resolved
get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution",
goal_index);
auto node = shared_from_this();
waypoint_task_executor_->processAtWaypoint(goal->poses[goal_index], goal_index);
Copy link
Member

@SteveMacenski SteveMacenski Sep 29, 2020

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

@jediofgever jediofgever Sep 30, 2020

Choose a reason for hiding this comment

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

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 ?

Copy link
Member

@SteveMacenski SteveMacenski Sep 30, 2020

Choose a reason for hiding this comment

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

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.

Copy link
Contributor Author

@jediofgever jediofgever Oct 5, 2020

Choose a reason for hiding this comment

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

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

nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 29, 2020

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
Copy link
Contributor Author

jediofgever commented Sep 30, 2020

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
Copy link
Member

SteveMacenski commented Sep 30, 2020

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.

doc/parameters/param_list.md Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

SteveMacenski commented Oct 3, 2020

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

@jediofgever
Copy link
Contributor Author

jediofgever commented Oct 3, 2020

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

@SteveMacenski
Copy link
Member

SteveMacenski commented Oct 4, 2020

No worries, enjoy your vacation 🌴

@naiveHobo
Copy link
Collaborator

naiveHobo commented Oct 4, 2020

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?

@@ -479,7 +479,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin
| ----------| --------| ------------|
| stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. |
| loop_rate | 20 | Rate to check for results from current navigation task |
| waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.|
| waypoint_task_executor_plugin | `WaitAtWaypoint` | Name of plugin to be loaded for executing waypoint tasks.|
Copy link
Contributor Author

@jediofgever jediofgever Oct 5, 2020

Choose a reason for hiding this comment

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

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

@jediofgever jediofgever changed the base branch from foxy-devel to main Oct 5, 2020
@jediofgever
Copy link
Contributor Author

jediofgever commented Oct 5, 2020

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-planning/navigation.ros.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
Copy link

codecov bot commented Oct 5, 2020

Codecov Report

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

Impacted file tree graph

@@             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.

@jediofgever jediofgever changed the base branch from main to foxy-devel Oct 5, 2020
@SteveMacenski
Copy link
Member

SteveMacenski commented Oct 5, 2020

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
Copy link
Contributor Author

jediofgever commented Oct 6, 2020

this PR has become somewhat of a mess

Agreed, but no worries I will handle it

SteveMacenski pushed a commit that referenced this pull request Oct 8, 2020
…iours at waypoint arrivals when using nav2_waypoint_follower (#2015)

* Add WaypointTaskExecutor plugin to main

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

* couple result of task execution with stop_on_failure

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

* add  failed ids to it's vector

* experimental change in nav2_waypoint_tester

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

* remove unused code
ruffsl pushed a commit to ruffsl/navigation2 that referenced this pull request Jul 2, 2021
…define behaviours at waypoint arrivals when using nav2_waypoint_follower (ros-planning#2015)

* Add WaypointTaskExecutor plugin to main

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

* couple result of task execution with stop_on_failure

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

* add  failed ids to it's vector

* experimental change in nav2_waypoint_tester

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

* remove unused code
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants