More info about the state of poses in NavigateThroughPoses

[tonynajjar]

Feature description

I have my application calling NavigateThroughPoses and using a BT similar to the demo one.
I would like to pass back to my application more information on the state of the poses as the action progresses; for example the poses removed by RemovePassedGoals would be "completed". The poses removed by "RemoveInCollisionGoals" would be "skipped/aborted".

This would be useful for the application node for various reasons, some examples:

• going back to the unsuccessful poses in a coverage application
• maintain a list of completed poses for pausing/resuming purposes
• visualizing the poses by color

Implementation considerations

The BT nodes would communicate this feedback with the navigator through the blackboard and the navigator would pass it back through the action's feedback channel. I'm not sure in what form though, it could be through separate values e.g. int[] skipped_poses or a supercharged wrapper around geometry_msgs/PoseStampedArray poses.

Let me know your general thoughts

[SteveMacenski]

I think having the feedback and result messages containing information about the waypoint status' would be valuable here and in the FollowWaypoints / FollowGPSWaypoints messages. Basically 3 (or 4?) arrays: successfully completed, skipped, and failed (and pending?) -- whereas each waypoint is added to one of each as it continues. Probably better though would be a new message type that itself WaypointStatus that contains an enum for those cases and we populate that and publish WaypointStatusArray within the action feedback/result. Also in that message perhaps add in the goal's pose just for good measure. That way you can for() over it and check the statuses more naturally and have the input goal poses in-order.

Populating the message in the NavThroughPose callbacks / WPF application is easy enough. I'm not entirely sure how to have the BT XML communicate it to the feedback/result Navigator plugins. Obviously the blackboard, but I think that requires a few new reserved blackboard keys for them to read (see goal, goals, path handling) and all of the respective BT nodes to populate them.

This could be a place to use the pre- and post- condition callbacks the BT.CPPv4 provides. If during a tick we change the goal/goals vector, then we check which BT node ticked did it and then update by a policy in the post- callbacks. I'm not 100% sure if that's possible in BT.CPPv4, nor if that's even the best idea, but just something to illustrate my point. It might be nice to not modify every single node that could possibly modify the goal vector (error prone; ambiguous code returns for a given node) and instead centralize the policy so not BT nodes need to be changed (and new BT nodes can be easily added to the policy).