Publishing frame using launch file/C++

Question

I'm trying to publish merged point cloud from multiple Kinects as one topic. In order to align clouds I applied static_transform_publisher, which merged all the clouds into one frame ("kinect"). Unfortunately, I need to publish this frame to save the resultant point cloud using pointcloud_to_pcd.

I've found that "pub" can be used to do it, but I'm confused about the arguments:

<node pkg="rostopic" type="rostopic" name="joined_kinects" args="pub /kinect "/>

If you can comment on how to improve the performance of such a system, please also share what can be applied (maybe using a bunch of C++ subscription code instead of a launch file), as currently in Rviz I get around 5 fps.

My launch file:

<launch>
<arg name="camera1_id" default="A22596V03205310A" />
<arg name="camera2_id" default="A22596V02246310A" />
<arg name="camera3_id" default="A22596V02323310A" />
<arg name="camera4_id" default="A22596V03345310A" />
<arg name="depth_registration" default="true" />

<arg name="camera1_name" default="kinect1" />
<arg name="camera2_name" default="kinect2" />
<arg name="camera3_name" default="kinect3" />
<arg name="camera4_name" default="kinect4" />
<rosparam>
   /use_sim_time : false
</rosparam>

<include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="device_id" value="$(arg camera1_id)" />
    <arg name="camera" value="$(arg camera1_name)" />
    <arg name="depth_registration" value="$(arg depth_registration)" />
</include>
<include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="device_id" value="$(arg camera2_id)" />
    <arg name="camera" value="$(arg camera2_name)" />
    <arg name="depth_registration" value="$(arg depth_registration)" />
</include>
<include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="device_id" value="$(arg camera3_id)" />
    <arg name="camera" value="$(arg camera3_name)" />
    <arg name="depth_registration" value="$(arg depth_registration)" />
</include>
<include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="device_id" value="$(arg camera4_id)" />
    <arg name="camera" value="$(arg camera4_name)" />
    <arg name="depth_registration" value="$(arg depth_registration)" />
</include>

<node name="kinect1_depth_optical_frame" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 kinect kinect1_depth_optical_frame 40" />
<node name="kinect1_rgb_optical_frame" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 kinect kinect1_rgb_optical_frame 40" />
<node name="kinect2_depth_optical_frame" pkg="tf" type="static_transform_publisher" args="-1.904 -1.109 1.611 1.5332 1.0410 0.9273 kinect kinect2_depth_optical_frame 40" />
<node name="kinect2_rgb_optical_frame" pkg="tf" type="static_transform_publisher" args="-1.904 -1.109 1.611 1.5332 1.0410 0.9273 kinect kinect2_rgb_optical_frame 40" />
<node name="kinect3_depth_optical_frame" pkg="tf" type="static_transform_publisher" args="1.987 -0.783 1.756 -1.6458 -1.0082 1.1499 kinect kinect3_depth_optical_frame 40" />
<node name="kinect3_rgb_optical_frame" pkg="tf" type="static_transform_publisher" args="1.987 -0.783 1.756 -1.6458 -1.0082 1.1499 kinect kinect3_rgb_optical_frame 40" />
<node name="kinect4_depth_optical_frame" pkg="tf" type="static_transform_publisher" args="-0.1735 -1.9762 3.3265 3.1083 0.1532 2.0799 kinect kinect4_depth_optical_frame 40" />
<node name="kinect4_rgb_optical_frame" pkg="tf" type="static_transform_publisher" args="-0.1735 -1.9762 3.3265 3.1083 0.1532 2.0799 kinect kinect4_rgb_optical_frame 40" />

<node name="rviz" pkg="rviz" type="rviz"/>
</launch>

---

Originally posted by Mikhail K. on ROS Answers with karma: 35 on 2018-07-05

Post score: 0

Answer 1

Just to clarify, you want your system to be able to save a pcd file which is a single cloud merging the point clouds from all four kinect sensors?

At the moment you're publishing 4 separate point clouds in different frames and defining a transformation tree which allows RVIZ to display them all at the same time. The point clouds are not being merged into a single cloud yet, the kinect frame is simply a node in the TF tree is not a point cloud at all.

To achieve what you want you'll need to write a node which subscribes to the four kinect point cloud topics and also sets up a transform listener to receive information about the TF tree. Then you'll need to synchronise the four incoming topics somehow, then transform the points in each point cloud so it is represented in the same frame and them to a new point cloud. You can then save this new merged point cloud to a PCD file.

This isn't the simplest thing and you'll need to get your hands dirty with python or C++ but I hope it gets you going in the right direction.

Edit: To do a quick dirty synchronisation you could store the last point clouds on each of the four topics. These point clouds would start out empty, then at the end of each callback you you check if all four of the stored point clouds contain points. This is a test to check that a cloud has been received on each topic. Then you could merge all four clouds and save, then clear the four saved clouds.

This method effectively processes the most recent four clouds that have not been processed already, and is tolerant of the clouds arriving at different rates and orders.

---

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-07-05

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Mikhail K. on 2018-07-05:
Thank you! Any suggestions how to synchronise the system? I used a "hacky" approach to take shots from two cameras, but this time I certainly need a separate thread for each camera. I'm not aware of a good guide on this topic

Comment by PeteBlackerThe3rd on 2018-07-05:
Do you need to save a single pcd of this scene or do you need to save a continuous stream of merged clouds? The former has a very simple solution the latter is a bit trickier.

Comment by Mikhail K. on 2018-07-05:
Well, I'm recording this data for deep learning, so I need as much data as I can get. Simple pointcloud_to_pcd even for one cloud operates at around 1hz

Comment by PeteBlackerThe3rd on 2018-07-05:
If need to record as quickly as possible, I'd recommend using a rosbag record onto a fast SSD on a decent computer, then playing it back at 1/10th - 1/30th real time to extract the PCD's

Comment by Mikhail K. on 2018-07-13:
I wrote a simple point cloud solution using MultiThreadedSpinner, and it works, but as soon as I try to save using pcl::io::savePCDFileASCII, it takes around 500ms to do it. Whereas rosbag records at desired 30hz, but a 10s recording weights 12GB. Any suggestions? Code: tinyurl.com/y8dwf7fp

Comment by PeteBlackerThe3rd on 2018-07-13:
The rosbag format is far more efficient than the ascii pcd format so I would expect the pcd's to end up being even more than 12GB. If there is a binary pcd format that should save much quicker and take up less space, I would recommend trying that.