Feature request: Provide "perceive"/"update" action?

[mintar]

It would be great if the pose_selector would provide a ROS action that the planner could call which enables DOPE, waits until it gets an update from DOPE (with a timeout of course), and then returns.

Currently, the planner calls the activate service, sleeps for 3 seconds in the hope that pose_selector will have received an update from DOPE during that time, then calls deactivate again:

https://github.com/DFKI-NI/mobipick_api/blob/246e7cde4cee8f76036c3ab01dfd41d3f8bc6acf/mobipick_api/perception.py#L65-L73

The problem is that either this sleep is too long (which slows down the demo) or too short (which leads to bugs like DFKI-NI/mobipick_labs#45). The proposed action would provide a feedback mechanism so that the planner can be sure that the pose_selector has received an update.

@marcvinci @alexander-sung : As we discussed on the Tuesday meeting, please coordinate with @AmosSmith3.

[AmosSmith3]

Ok yes, I can add this feature. Just a few clarification questions:

1. With the action, should the pose selector still take the DOPE result, convert it to a object_pose_msgs::ObjectList and save this converted result in pose_selector? Or should the DOPE result be returned in the action result? Or both?
2. Is there any feedback info that should be used in the action? I.e. current time spent waiting on DOPE? From your description it seems an action with empty goal/result/feedback fields would satisfy your needs, but perhaps additional info could be useful?

[mintar]

1. With the action, should the pose selector still take the DOPE result, convert it to a `object_pose_msgs::ObjectList` and save this converted result in `pose_selector`?

Yes, exactly this. The pose_selector should behave exactly as before, only that the "activate / wait / deactivate" now happens on the pose_selector side, not on the client side.

Or should the DOPE result be returned in the action result? Or both?

For the current use case, an empty action result would be fine; but if it's easy to do, returning the DOPE result (converted to object_pose_msgs::ObjectList) would be even better.

2. Is there any feedback info that should be used in the action? I.e. current time spent waiting on DOPE? From your description it seems an action with empty goal/result/feedback fields would satisfy your needs, but perhaps additional info could be useful?

No, I can't think of any meaningful action feedback.

BTW: Only do this if you want to. This issue came up in our last Tuesday meeting, and I wanted to keep track of it because I've been thinking about this several times now and I think "somebody" should do it, but that somebody doesn't have to be you! :)

Also, it might be a bit tricky regarding threading. I'm afraid if you use the SimpleActionServer, it will block inside the action feedback, so the DOPE callback will never be called and therefore the action will wait forever for a DOPE update. If that's the case, then either we have to

1. use a MultiThreadedSpinner (and make all the code thread-safe), or
2. do some more intricate action goal handling ourselves.

I guess option 1 is easier. Anyhow, just a warning, maybe it's not a problem.

[AmosSmith3]

Sorry for the delay on this, but I have finally gotten a chance to look at this in more detail.

My current idea for a solution to this would be to have the perceive action server in pose_selector:

1. When a perceive action goal is sent to this perceive action server, the server will then use a ros::topic::waitForMessage on the DOPE output topic, with a timeout duration. From some initial testing, this would seem to trigger the DOPE on-demand publishing.

2. Once a DOPE message is received (if no timeout occurs), the server would then process the DOPE vision_msgs::Detection3DArray result into the object_pose_msgs::ObjectList type (thereby bypassing the current DOPE callback setup, which I believe you were referring to in regards to potential issues with blocking?)

3. Once the DOPE output is converted into object_pose_msgs::ObjectList, it will be returned as the action result and also be used to update the current entries in pose_selector.

Does this seem like a valid way forward, or do you have any other feedback? If this looks good, let me know and I should have this completed soon and ready for some testing/review.

[mintar]

Yes, this sounds exactly right!

the server will then use a ros::topic::waitForMessage on the DOPE output topic, with a timeout duration. From some initial testing, this would seem to trigger the DOPE on-demand publishing.

Yup! All that waitForMessage does is creating a subscriber, waiting for a message, then closing the subscriber again. Since it subscribes, the DOPE on-demand publishing gets triggered.

(thereby bypassing the current DOPE callback setup, which I believe you were referring to in regards to potential issues with blocking?)

Yes, that's what I mean. Has to be tested though, perhaps it's not an issue.

Some things to test/look out for:

• Is it possible to have two subscribers (the one inside waitForMessage and the regular one) to the same topic in one node? (should be ok)
• If the action server callback is indeed blocking, the DOPE message that was triggered by waitForMessage should be queued in the regular subscriber's callback queue, so once the action server callback exits, the regular callback should get called again, so this DOPE message will be processed twice.