Can monitored planning scene subscribe to multiple point clouds

Question

System: Ubuntu 16.04 / ROS Kinetic / Nuvo-5095GC-022 / Universal Robots UR10 Packages: freenect_stack, geometry2, libfreenect, perception_pcl, trac_ik, universal_robot, ur_modern_driver

Hi,

As part of a dynamic collision avoidance project I'm trying to achieve full visibility of a live scene using multiple Kinect cameras. In the current setup I have two Xbox Kinect V1s, each producing a working point cloud. The cameras are positioned with a 90deg offset and the frames have been linked using tf2, so the points match up relatively well in 3D (cameras are intrinsically calibrated). From there I want to use both point clouds to create an octomap that the robot will plan to avoid.

My question: Is it possible to have the planning scene monitor subscribe to both point cloud 2 topics so it can create the octomap? Or do I have to concatenate the two point clouds using PCL before the planning scene monitor can read a single combined point cloud and create the octomap?

Thanks in advance!

---

Originally posted by nisur on ROS Answers with karma: 28 on 2018-10-09

Post score: 0

Answer 1

First, something to clarify perhaps:

Is it possible to have the planning scene monitor subscribe to

The planning scene does not subscribe to anything, it is either the PointCloudOctomapUpdater or the DepthImageOctomapUpdater that is responsible for updating the planning scene with information coming from either point clouds or depth images.

With that out of the way:

Is it possible to have the planning scene monitor subscribe to both point cloud 2 topics so it can create the octomap?

No, both plugins only subscribe to a single topic (here for the PointCloudOctomapUpdater). So they can't subscribe to multiple topics.

Or do I have to concatenate the two point clouds using PCL [..]

I don't believe this should be necessary: if you have both camera drivers publish on the same topic, and your TF is setup correctly, the updater plugin should be able to do all of the work with the two clouds.

---

An alternative might be to try and see whether it is possible to configure multiple instances of the same plugin in the sensors .yaml file. Each of the instances could listen to a different topic.

See Perception Pipeline Tutorial for the general tutorial.

I'm not sure what would be better, performance wise. I would actually be very interested to hear your experiences.

---

Something to keep in mind though: in the past the updater plugins locked the planning scene for every update they did. This locks out the rest of MoveIt from using the planning scene, leading to delays in planning. With high enough frame rates it could even lead to starvation of the motion planning part.

With two drivers publishing to the same topic this might become even more of an issue as you now potentially have 2xFPS compared to a situation with only a single camera.

I'm not sure you will run into this issue, or whether things have improved significantly since I last checked, but it may be something to be careful with.

---

Originally posted by gvdhoorn with karma: 86574 on 2018-10-09

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by nisur on 2018-10-09:
Thanks very much @gvdhoorn , really appreciate the level of detail (and the corrections!) in your answer.

Using multiple plugin instances in the yaml file sounds like the simplest method, however I'll try both out and will post an update if I can come to a conclusion regarding performance. Cheers!

Comment by gvdhoorn on 2018-10-30:
So what did you end up doing?

Comment by nisur on 2018-10-30:
Apologies for the late response. I haven't had chance to test out both options suggested, however i've found using multiple instances of the PointCloudOctomapUpdater to work as well as can be expected given the large volume of data being processed.