@SteveMacenski Could you add a brief summary or description of your approach here?

Yes, sorry I thought the discussion from before had enough description.

The goal here is to push the application-level complexity of multiple-waypoint following up a level. This is a basic prototype that can have a bunch of features like preempt-at-radius to it cleanly and isolated from the larger navigation stack or navigation task.

Adding those capabilities to navigation makes the navigation task more complex and "smart" to have to know about other tasks in the queue and push a bunch of parameters into the stack about what to do on failure and overall relatively limiting. This application is a step higher and easier to push complexity here -- or fork for user specific treatment of goals without having to mess with the core navigation action. This is an "example application" of using the navigation action to follow some waypoint set given.

A simple demo of this would be to load a yaml file with waypoints and call this action to follow them. I will plan on adding the yaml parser and loader server in here too if this is the direction we want to do. That will be useful for testing and basic robot deployment.

Hi @SteveMacenski

I like this solution. I thought that using the bt_navigator was mandatory. This makes it more simple to solve this problem.

I have to try this PR. If the robot does not stop and orientate in each wp, it is almost done.

Thanks!! Best
Francisco

If the robot does not stop and orientate in each wp, it is almost done.

Its going to do that right now, but it would be easy (and could be added after this is merged) to check the robot position over the action and then preempt with a new goal once you get inside a radius.

Knowing that that was going to be a need, I think having the separate action makes a ton of sense so the logic within the direct navigation code doesn't get too muddled up. It's also now an example of how to use the navigation action server (which ROS1 never had)

If you like this, feel free to close your other PR and we can work off of this. I also still need to hear from the Intel guys if they like this.

@SteveMacenski @fmrico let's discuss this at the next WG meeting (in a few hours).

Codecov Report

❗ No coverage uploaded for pull request base (master@56df144). Click here to learn what that means.
The diff coverage is 16.45%.

@@            Coverage Diff            @@
##             master    #1338   +/-   ##
=========================================
  Coverage          ?   40.42%           
=========================================
  Files             ?      233           
  Lines             ?    11077           
  Branches          ?     4673           
=========================================
  Hits              ?     4478           
  Misses            ?     3589           
  Partials          ?     3010

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 56df144...b4c34d3. Read the comment docs.

Before this is approved, can you add:

1. return a result
2. rate as a parameter
3. documentation
4. a system test for this

Yes to all of above

Just a few changes and will match option 1 from #803.

@fmrico added the necessary changes off your panel changes to make it work with the new action server and state.

@mkhansen-intel added the throttle parameter, readme, return result

Missing integration test, writing now but its good to review otherwise. Tested against rviz updates and CLI.

Hi @SteveMacenski

Ouch, I was waiting for this PR to make another PR with the rviz plugin. I had been working on it this weekend.

In any case, I also added the visualization using visual markers. You can take it from here.

Best

@fmrico it was also a little more involved to update for this PR since there's a second action server and goal handles for this method. With that said the clean state machine made it straight forward.

I can leave out the marker displays for this and you can PR it after this is in. I want to finish the integration tests this afternoon and have this merged by week's end.

I was going to make a yaml parser waypoint version but your rviz tool made it much easier, thank you

Also I know you wanted the preemption stuff, another good add on from this PR.

I didn't add that here and I don't plan to to not try to make a mega-PR of too much "stuff".

@SteveMacenski Perfect. If you finish by week's end, I can add the markers during the weekend.

Thanks!!

System test is up and running.

gif of test

/bin/bash: install/setup.sh: No such file or directory

Ignore the debug test, its that weird issue unrelated

@mkhansen-intel and others - can you review again?

@mkhansen-intel should we get a second review on this?

Yes, unfortunately Carlos is out this week. Can it wait until next week or do you want someone else to review it?

I don't mind who look at it. It can also wait as long as its before the E sync

We'll ask Carlos to look at it again Monday since this is a short week anyway.

Waiting on CI from updated typedef then merging

Tests fail from CI problems not PR problems. Its the no install/setup.bash error