More flexible target definition

[reinzor]

Hi,

Thanks for this awesome extension to MoveBase / ROS Nav stack. I just watched your ROSCON video and I did really like it; especially since we are using SMACH for our state behavior as well together with a local and global planner interface (using the nav_core planners). I thing we encountered during the Robocup@Home competitions is that we would like to express the navigation targets with use of contraints. I see that you are still using the geometry_msgs/PoseStamped. What do you think about this?

Our implementation can be found here:

https://github.com/tue-robotics/cb_base_navigation

Would love to hear your thoughts. Thanks again!

[corot]

Sounds interesting, but I cannot easily find use cases for those constraint-based goals. Do you have any examples?
Also, the typical planners I know about work with start and goal poses, optionally with some tolerance. And on MBF we are also introducing waypoints the planner should pass by (btw, would you find this interesting?). So to reach a constraint-based goal you would need either to modify heavily the planner, or to calculate the easiest-to-reach goal satisfying all the constraints. For this second case, MBF can help, as you can have an SMACH task that performs this calculation and the feed it to the global planner task.

[reinzor]

Thanks for your response. Waypoints are definitely interesting but I think adding these waypoints to a higher layer (e.g. topological layer) makes more sense to me.

Regarding the constraints, some examples:

• In front of a table (defined by a rectangular region)
• Following a peron (defined by a radius around the person)
• Grasping object: inverse reachability of robot end effector,

These examples can all not really be described by a single pose. I agree that adding this to the internals would cause a break in the nav_core api and adding this as a preprocessor is easier. However, navigating to a room without stopping for example, cannot be expressed

[corot]

I see... interesting use cases. But how looks a planner pursuing such goals? I can imagine NavFn creating an area of minimum cost where all the constraints are satisfied... but what about sampling based planners, as RTT*? or search based as SBPL? Thinking a bit, yes, should not be that difficult sampling based, just spread goal states along the constraints-satisfying area.

Anyway, if you are you considering to integrate your work on MBF, you are more than welcome! But current development is going on in single_int_iface branch, and we plan to rebase to master soon (and make a first release)

[reinzor]

That's great, I will keep following your updates and plan to play with your implemenation soon. And, if I have the cycles, I will look at integrating the work. However, it will definitely break the API so more discussion would be required for sure

[corot]

Breaking nav_core API is not an issue, as we already break it with MBF (hence the move_base_flex_core package. We wrap old plugins for backward compatibility, so in case of calling the global planner with a constraint-based goal when using an old plugin, will return NO_IMPLEMENTED or something like that.

[spuetz]

I like the idea of constraint-based goal and start definitions for computing plans. We could define the start and the goal for a frame, which could be dynamic and moving, and we could find a way of defining areas / volumes around a position.
And maybe we could add an interface for scoring the result which then could be used for reasoning tasks.

[spuetz]

So I looked into your code here: https://github.com/tue-robotics/cb_base_navigation and here: https://github.com/tue-robotics/cb_planner_msgs_srvs/tree/master/msg
I think we can do it a bit more clear using the standard msg types. I would rather prefer a list of bounding boxes (two geometry_msgs/PointStamped) which is connected to a frame. And the we need optional possible angles...or a list angle ranges for each bounding box.

[reinzor]

I do agree, however, not all contraints that we required in robocup could be expressed with use of boundingboxes. We also have circular constraints and areas around arbitrarily shaped objects.

…

-Rein

On Mon, Feb 26, 2018 at 11:34 AM, Sebastian Pütz ***@***.***> wrote: So I looked into your code here: https://github.com/tue- robotics/cb_base_navigation and here: https://github.com/tue- robotics/cb_planner_msgs_srvs/tree/master/msg I think we can do it a bit more clear using the standard msg types. I would rather prefer a list of bounding boxes (two geometry_msgs/PointStamped) which is connected to a frame. And the we need optional possible angles...or a list angle ranges for each bounding box. — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#8 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AD-4lzG3vuqhNQTeAy21yyegNrTY4s0bks5tYojKgaJpZM4QHPbu> .

[corot]

To my understanding, these goals will be passed to the global planner; so they must be either something generic enough for the typical planner to operate with (I can imagine a list of poses, or a polygon defining an area), or if not MBF must have a dedicated module that converts your message with formulas into something like that.

[reinzor]

​I think this is a good idea, as long as the goal definitions are generic enough to define goal regions for the constraint. But maybe you also want to add some optimization function in some way. Ideally you would define some kind of cost landscape i.m.o. However, it will be hard to fit this in the common_msgs structures. What are your thought on goal optimization if we have goal area definitions with use of polygons or other shapes.​

…

-Rein

On Wed, Feb 28, 2018 at 9:39 AM, Jorge Santos Simón < ***@***.***> wrote: To my understanding, these goals will be passed to the global planner; so they must be either something generic enough for the typical planner to operate with (I can imagine a list of poses, or a polygon defining an area), or if not MBF must have a dedicated module that converts your message with formulas into something like that. — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#8 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AD-4l1Ph9Sqgp03yJe-tb3ei_Xc024Kqks5tZRCugaJpZM4QHPbu> .

[spuetz]

polygons are a problem in 3D ;)

[reinzor]

convex or concave hulls?

…

-Rein

On Wed, Feb 28, 2018 at 1:00 PM, Sebastian Pütz ***@***.***> wrote: polygons are a problem in 3D ;) — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#8 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AD-4lxHF57h-OwgN8ItXMua9Vr3MiSMLks5tZT_EgaJpZM4QHPbu> .

[corot]

my problem with those fancy equations or cost functions is that I can hardly imagine how to implement then in the typical planner. I can imagine a surface, so all the grids inside are valid goals for, for example, Navfn. So equations would be fine as long as they define a surface. But optimization function... not sure what do you mean, and much less how to send as a goal to a typical planner.

[reinzor]

For me the goal definition is not clear yet but the use case is. For example if you would like to navigate to a room, you always have a desired position (maybe the center point) (optimization) but the constraint is in the room. In our Robocup @Home use cases we often have these problems. So a goal definition could be an area with a desired point in that area that can be used for optimization and give the robot x time to optimize if the constraint has been met. This is still a little vague but these are some thoughts.

…

-Rein

On Thu, Mar 1, 2018 at 10:26 AM, Jorge Santos Simón < ***@***.***> wrote: my problem with those fancy equations or cost functions is that I can hardly imagine how to implement then in the typical planner. I can imagine a surface, so all the grids inside are valid goals for, for example, Navfn. So equations would be fine as long as they define a surface. But optimization function... not sure what do you mean, and much less how to send as a goal to a typical planner. — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#8 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AD-4l17zUNnBC5aMUQlAXCxzNTCIaIf0ks5tZ77igaJpZM4QHPbu> .

[corot]

We are considering adding a service that, given a pose and xy / yaw tolerances, will return the closest pose within tolerance thresholds (or fail if none exists). True, doesn't cover all the ideas bounced here, but goes in that direction.

Draft PR with the proposed interface, to gather some feedback: #316