Feature - load, save and loop waypoints by padhupradheep · Pull Request #3165 · ros-navigation/navigation2

padhupradheep · 2022-09-01T18:01:41Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #2826 )
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simulation of Neobotix

Description of contribution in a few bullet points

Added new feature to load and save waypoints in a yaml file for waypoint following
Added the loop functionality to revisit the waypoints
Pause and resume button for stopping and continuing through the waypoints.

Description of documentation updates required from your changes

Add an example yaml file
Description about the functionality

Future work that may be required in bullet points

Update the pause status in the plugin, currently the status reads cancelled when the robot is paused in between the waypoints (will be done as part of this PR).

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

mergify · 2022-09-01T18:02:15Z

@padhupradheep, please properly fill in PR template in the future. @SteveMacenski, use this instead.

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

padhupradheep · 2022-09-01T18:06:31Z

Still have some work left such as updating the status for pausing in the RViz plugin and renaming the variables and / or functions. Hope to complete it on the weekend.

nav2_msgs/action/FollowWaypoints.action

SteveMacenski

I think the loop stuff in the WP follower is great!

nav2_rviz_plugins/src/nav2_panel.cpp

nav2_waypoint_follower/src/waypoint_follower.cpp

SteveMacenski · 2022-09-22T17:39:43Z

any update?

padhupradheep · 2022-09-22T17:45:54Z

Not yet, still away on PTO, hopefully this should be up by mid October. Sorry for the delay.

SteveMacenski · 2022-09-22T17:48:43Z

OK! Wow, what do I need to do to get that kind of PTO 😉

Hey, no worries, enjoy vacation! Sorry to bother.

padhupradheep · 2022-10-10T05:55:51Z

FYI, I'm back from my PTO. Hopefully would finish it ASAP.

Just doing a rebase, so that I can catch up with all the changes made in the last month or so.

padhupradheep · 2022-10-11T18:12:06Z

Okay now handling everything in the panel level. Now in the below video you can see that I'm able to select waypoints, set number of loops, start waypoint following and pausing the waypoint following at the middle and resuming it again.

Additionally you can also see, there is a status on the panel saying at which waypoint and at which loop the current navigation is happening, also when you press the pause button, you also get a note saying that the last cancelled waypoint is stored, which could be resumed.

save_load-2022-10-11_20.00.29.mp4

It is not still yet perfect (need to solve edge cases), but just wanted to know, whether will this additional status stuff be interesting for you / users ? what do you think ?

(sorry for the poor quality video and simulation performance, for some reason my graphics card is behaving weird in the 22.04, which slows down RViz a lot.)

nav2_rviz_plugins/src/nav2_panel.cpp

SteveMacenski · 2022-10-11T18:48:31Z

Yeah! Super useful! Question: if you have looping of waypoint following where you set the poses via rviz (not a file), does the initial robot point count as a starting point? That way if you loop you have the initial segment included as well? Should it? I'm not sure, but seems reasonable.

Still not a fan of the goal_index as an entry in the waypoint follower - but I can live with it if we set it to 0 by default in the msg definition so if no one fills it in, it explicitly goes to use the full list

padhupradheep · 2022-10-11T19:01:12Z

if you have looping of waypoint following where you set the poses via rviz (not a file), does the initial robot point count as a starting point?

No, If the user wants to use the initial starting point as well, then he has to select the initial starting pose as like the rest of the poses. I initially thought about adding the initial pose as part of the waypoint list, then anyways if the user wants it, then he's just a drag away.

That way if you loop you have the initial segment included as well? Should it? I'm not sure, but seems reasonable.

I feel, this depends more on the use-case that the user wants to handle. Say if the user is starting from a docking area, then he definitely does not want the starting point included.

Maybe we can just handle both the cases by having a check box for selecting if the start pose needs to be included or not. Leaving the choice to the users.

Still not a fan of the goal_index as an entry in the waypoint follower - but I can live with it if we set it to 0 by default in the msg definition so if no one fills it in, it explicitly goes to use the full list

goal_index has been removed from the waypoint follower, I'm now handling everything in the panel level. I just haven't pushed the changes. 😅

SteveMacenski · 2022-10-11T19:23:58Z

Maybe we can just handle both the cases by having a check box for selecting if the start pose needs to be included or not. Leaving the choice to the users.

I like that

goal_index has been removed from the waypoint follower, I'm now handling everything in the panel level. I just haven't pushed the changes. sweat_smile

Oh? It was starting to grow on me a bit 😉

padhupradheep · 2022-10-12T18:12:42Z

You can avoid looking at the recent commit and providing a review for it, still need to clean out few things and test for edge cases.

I'll let you know, once I'm done.

Meanwhile, I added the check for including the initial_pose

mergify · 2022-10-12T18:18:08Z

@padhupradheep, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2022-10-12T19:02:47Z

Nice!

I do think the looping feature of the WP follower is valuable, I would actually like that logic back into the WPF server if possible. I can see that being a common feature request even outside of this navigation panel tool.

padhupradheep · 2022-10-12T19:10:42Z

I do think the looping feature of the WP follower is valuable, I would actually like that logic back into the WPF server if possible.

No problem!

Only thing is that, we also need the ‘goal_index’ back as well, that is very much essential for the waypoint follower to know about the pausing. As you said earlier in this thread I can set it’s default to 0.

SteveMacenski · 2022-10-12T20:36:27Z

Ok!

nav2_rviz_plugins/src/nav2_panel.cpp

nav2_waypoint_follower/src/waypoint_follower.cpp

padhupradheep · 2022-10-13T18:26:45Z

Would be nice to have some user feedback, before marking it ready for review.

Our nice friend @jwallace42 has agreed to test it and provide some feedback tomorrow. Meanwhile, I'll also test it a bit more, making sure, there are no edge cases.

jwallace42 · 2022-10-14T18:10:01Z

Looks pretty good :). Here is what I found.

I can crash the waypoint server by trying to follow zero waypoints. We should protect against this and just fail the action right away.
If I hit the pause waypoint button then cancel it stays stuck and I cannot start a new way point following. If I don't pause and just cancel, things work as expected.
Before the robot is localized at the start the save wappoints, load wappoints and pause waypoints boxes have no text in them.
Server crashes if I put a huge number for loops or something that is not a number :) I think you need to bound the inputs to only positive numbers within a range.
I am also able to change the loop number in the text box during nav. if you can gray that out that would be good.
Way points can be put in invalid places. (outside map, LETHAL area)
If I pause the waypoint, should be able to save the waypoints.

jwallace42 · 2022-10-14T18:10:35Z

Also,

pull in main :).

padhupradheep · 2022-10-14T19:04:45Z

Once again, thanks for the nice feedback. @jwallace42

I am also able to change the loop number in the text box during nav. if you can gray that out that would be good.

do you mean during nav_through_poses?

Edit: Never mind! Found it!

codecov · 2022-10-14T19:43:04Z

Codecov Report

Base: 89.03% // Head: 88.97% // Decreases project coverage by -0.06% ⚠️

Coverage data is based on head (c296463) compared to base (59f5a51).
Patch coverage: 88.88% of modified lines in pull request are covered.

❗ Current head c296463 differs from pull request most recent head 275ff88. Consider uploading reports for the commit 275ff88 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3165      +/-   ##
==========================================
- Coverage   89.03%   88.97%   -0.07%     
==========================================
  Files         351      351              
  Lines       15658    15662       +4     
==========================================
- Hits        13941    13935       -6     
- Misses       1717     1727      +10

Impacted Files	Coverage Δ
nav2_waypoint_follower/src/waypoint_follower.cpp	`82.58% <88.88%> (-0.87%)`	⬇️
nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp	`75.00% <0.00%> (-12.50%)`	⬇️
...2_core/include/nav2_core/controller_exceptions.hpp	`60.00% <0.00%> (-10.00%)`	⬇️
..._planner/include/nav2_smac_planner/node_hybrid.hpp	`92.85% <0.00%> (-7.15%)`	⬇️
...stmap_2d/plugins/costmap_filters/binary_filter.cpp	`94.80% <0.00%> (-2.60%)`	⬇️
nav2_controller/src/controller_server.cpp	`85.71% <0.00%> (-1.91%)`	⬇️
...tree/include/nav2_behavior_tree/bt_action_node.hpp	`84.21% <0.00%> (-1.76%)`	⬇️
nav2_costmap_2d/src/layered_costmap.cpp	`95.49% <0.00%> (-0.91%)`	⬇️
nav2_navfn_planner/src/navfn_planner.cpp	`96.90% <0.00%> (+1.03%)`	⬆️
... and 1 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

padhupradheep · 2022-10-17T20:10:01Z

I have added couple of additional checks to verify the specified waypoints are valid and there are no obstacle in that particular point in the map and also checked if the waypoints are specified inside the map bounds.

@jwallace42 I think I have handled all your points and most of the other edge cases, do you mind testing it once again and giving again your feedback if you have time? Your insights were really helpful last time around. Nothing urgent though, anyways Steve is away for ROSCon.

jwallace42 · 2022-10-18T14:00:29Z

@padhupradheep
Yeah,

I will test this today and if all goes well I will review the code :).

Do you have a docs pr up?

…ered footprint collsion

…mplete

…tion to take it from params or the default

SteveMacenski

I'm happy with this! Anything else before merging?

You should post on discourse about this!

padhupradheep · 2023-01-05T20:02:33Z

I'm happy with this! Anything else before merging?

I was actually looking for something to update in docs. But I think the plugin is easy to understand, there is not much of a trick there.

You should post on discourse about this!

Sure, I'll take it to our community..

SteveMacenski · 2023-01-05T21:48:18Z

I think a migration guide entry just to acknowledge the changes would be good!

I'm also going to re-run CI, I see the WPF test failed and given there are substantive changes there, its important that passes

SteveMacenski · 2023-01-06T18:57:09Z

@padhupradheep definitely post on discourse with a nice video / gif of this is action!

padhupradheep · 2023-01-06T21:29:59Z

definitely post on discourse with a nice video / gif of this is action!

Sure!

Thanks for the consistent motivation and bearing with me through the feature design. 😃

SteveMacenski · 2023-01-06T21:44:27Z

No worries at all, glad to have this in! Only took... 3 months largely due to my delays in reviews 😆

* able to loop, load and save wp using json * replacing the file format to yaml from json * feedback received and ready to be handled for pause and resume * handling loops in waypoint follower server and adding the corresponding variables in the waypoint action * handling goal_index * adding pause and resume functionality for waypoint following * handling loops and making it available only for nav through poses * handling exceptions when invalid files are loaded * input the file name in which waypoints need to be saved * satisfying linters * update * publish waypoints after loading * add logs for the loops * handling everything at the panel level * fix missing * handling loops from the wp follower * fixing linters * removing rclcpp expression * setting the default of goal_index to 0 * adding comments * correction * Update nav2_waypoint_follower/src/waypoint_follower.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * update review * add the missing feedback * fixes * adding a pause state indicator, handling zero waypoints, handling cancel during pause * greying out number of loops during nav_through_poses * handling edge cases * adding option to save waypoints while paused * checking for the validity of waypoint, if it is inside the map and not in an obstacle w.r.t global costmap * Fix pre_initial states * solving further edge cases * sanity check for the loaded waypoints * clearing loaded acumulated poses, when navigating to single goal * function name change * renaming variable * handling invalid punctuation * making costmap_subs as uniqur_ptrs * checking if the selected waypoints will be in collision - also considered footprint collsion * jumping to accumulating state once waypoint or nav_through_pose is complete * updates for handling 1st goal in the 1st loop + fixed pausing issue * updates after review + adding small logo * review fix - 1 * map frame is derived from accumulated_poses and base_frame has the option to take it from params or the default * removing mistakes from merge * re-add reset for navigation feedback indicator * minors * review update Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

…ion#3165 ros-navigation#3354

…s-navigation#3354) Signed-off-by: TzuShianYang <tzushian@cavedu.com>

padhupradheep changed the title ~~Feature/koad save loop wp~~ Feature - load, save and loop waypoints Sep 1, 2022

SteveMacenski reviewed Sep 1, 2022

View reviewed changes

nav2_msgs/action/FollowWaypoints.action Outdated Show resolved Hide resolved

nav2_msgs/action/FollowWaypoints.action Show resolved Hide resolved

SteveMacenski reviewed Sep 6, 2022

View reviewed changes

nav2_rviz_plugins/src/nav2_panel.cpp Outdated Show resolved Hide resolved

nav2_waypoint_follower/src/waypoint_follower.cpp Show resolved Hide resolved

nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved

padhupradheep force-pushed the feature/koad-save-loop-wp branch from 87613aa to 74374f3 Compare October 10, 2022 09:57

SteveMacenski reviewed Oct 11, 2022

View reviewed changes

nav2_rviz_plugins/src/nav2_panel.cpp Outdated Show resolved Hide resolved

SteveMacenski reviewed Oct 13, 2022

View reviewed changes

nav2_rviz_plugins/src/nav2_panel.cpp Outdated Show resolved Hide resolved

nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved

padhupradheep force-pushed the feature/koad-save-loop-wp branch from 97c579d to 405803d Compare October 14, 2022 18:52

padhupradheep force-pushed the feature/koad-save-loop-wp branch from 05a0c85 to 68abd75 Compare October 17, 2022 10:05

padhupradheep added 13 commits January 2, 2023 13:00

sanity check for the loaded waypoints

8279f9e

clearing loaded acumulated poses, when navigating to single goal

1650340

function name change

fd5826f

renaming variable

dfff428

handling invalid punctuation

5ac5e24

making costmap_subs as uniqur_ptrs

1a9b2c2

checking if the selected waypoints will be in collision - also consid…

6a4ce1f

…ered footprint collsion

jumping to accumulating state once waypoint or nav_through_pose is co…

9d5538b

…mplete

updates for handling 1st goal in the 1st loop + fixed pausing issue

a12cf33

updates after review + adding small logo

d471b6f

review fix - 1

ded776a

map frame is derived from accumulated_poses and base_frame has the op…

c0ec0e6

…tion to take it from params or the default

removing mistakes from merge

679b90a

padhupradheep force-pushed the feature/koad-save-loop-wp branch from 642d03e to 679b90a Compare January 2, 2023 12:04

padhupradheep added 3 commits January 2, 2023 15:24

re-add reset for navigation feedback indicator

c3d91c1

minors

3d612b2

review update

2c79299

padhupradheep mentioned this pull request Jan 4, 2023

logging and displaying time taken for completing the waypoint following in nav2 rviz panel #3347

Closed

SteveMacenski approved these changes Jan 4, 2023

View reviewed changes

padhupradheep mentioned this pull request Jan 6, 2023

add docu for load, loop save waypoint in RViz panel ros-navigation/docs.nav2.org#383

Merged

SteveMacenski merged commit 63e4072 into ros-navigation:main Jan 6, 2023

padhupradheep mentioned this pull request Jan 9, 2023

fixing pose orientations while loading #3354

Merged

xerathyang pushed a commit to xerathyang/navigation2 that referenced this pull request Feb 16, 2024

Backport nav2_rviz_plugins updates to humble version from ros-navigat…

2fae815

…ion#3165 ros-navigation#3354

xerathyang pushed a commit to xerathyang/navigation2 that referenced this pull request Feb 17, 2024

Backport feature: save, load, loop waypoints (ros-navigation#3165, ro…

a57b811

…s-navigation#3354) Signed-off-by: TzuShianYang <tzushian@cavedu.com>

Conversation

padhupradheep commented Sep 1, 2022

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Sep 1, 2022

Uh oh!

padhupradheep commented Sep 1, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Sep 22, 2022

Uh oh!

padhupradheep commented Sep 22, 2022

Uh oh!

SteveMacenski commented Sep 22, 2022

Uh oh!

padhupradheep commented Oct 10, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

padhupradheep commented Oct 11, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Oct 11, 2022

Uh oh!

padhupradheep commented Oct 11, 2022

Uh oh!

SteveMacenski commented Oct 11, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

padhupradheep commented Oct 12, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mergify bot commented Oct 12, 2022

Uh oh!

SteveMacenski commented Oct 12, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

padhupradheep commented Oct 12, 2022

Uh oh!

SteveMacenski commented Oct 12, 2022

Uh oh!

Uh oh!

Uh oh!

padhupradheep commented Oct 13, 2022

Uh oh!

jwallace42 commented Oct 14, 2022

Uh oh!

jwallace42 commented Oct 14, 2022

Uh oh!

padhupradheep commented Oct 14, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Oct 14, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

padhupradheep commented Oct 17, 2022

Uh oh!

jwallace42 commented Oct 18, 2022

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

padhupradheep commented Jan 5, 2023

Uh oh!

SteveMacenski commented Jan 5, 2023

Uh oh!

SteveMacenski commented Jan 6, 2023

padhupradheep commented Sep 1, 2022 •

edited

Loading

padhupradheep commented Oct 10, 2022 •

edited

Loading

padhupradheep commented Oct 11, 2022 •

edited

Loading

SteveMacenski commented Oct 11, 2022 •

edited

Loading

padhupradheep commented Oct 12, 2022 •

edited

Loading

SteveMacenski commented Oct 12, 2022 •

edited

Loading

padhupradheep commented Oct 14, 2022 •

edited

Loading

codecov bot commented Oct 14, 2022 •

edited

Loading

SteveMacenski commented Jan 6, 2023 •

edited

Loading