Hi all,
I am trying to merge two pointclouds from kinects, convert this merged pointcloud data into 2D image and publish the image data as a topic. I am using ros fuerte.
Here is my launch file:
<launch>
<!--broadcasting the transformation from world frame to kinect frames-->
<node pkg="tf" type="static_transform_publisher" name="overhead_kinect1" args="-0.37 0.94 1.04 0 0.83 -1.57 /world_base_link /overhead_kinect1_link 40" />
<node pkg="tf" type="static_transform_publisher" name="overhead_kinect2" args=" 0.37 0.98 1.02 0 0.80 -1.57 /world_base_link /overhead_kinect2_link 40" />
<include file="$(find openni_launch)/launch/openni.launch">
<arg name="camera" value="overhead_kinect1"/>
<arg name="depth_registration" value="false" />
<!--<arg name="device_id" value="A00365911792039A"/>-->
<arg name="device_id" value="002@18"/>
</include>
<include file="$(find openni_launch)/launch/openni.launch">
<arg name="camera" value="overhead_kinect2"/>
<arg name="depth_registration" value="false" />
<!--<arg name="device_id" value="000000000000000"/>-->
<arg name="device_id" value="002@20"/>
</include>
<!--
<node pkg="rviz" type="rviz" name="rviz"
args="-d $(find ar_marker_detection_multiple)/ar_marker_detection.vcg"/>
-->
<node pkg="ar_marker_detection_multiple" type="ar_kinect" name="ar_kinect" respawn="false" output="screen"/>
</launch>
and part of the code responsible for merging:
ros::Publisher pub;
tf::TransformListener *listener;
sensor_msgs::PointCloud2 output, output1, output2;
pcl::PointCloud<pcl::PointXYZ> output_pcl, output1_pcl, output2_pcl;
void cloud_cb1(const sensor_msgs::PointCloud2ConstPtr& input1) {
listener->waitForTransform("/world_base_link", (*input1).header.frame_id, (*input1).header.stamp, ros::Duration(5.0));
sensor_msgs::PointCloud &pcout) const
pcl_ros::transformPointCloud("/world_base_link", *input1, output1, *listener);
pcl::fromROSMsg(output1, output1_pcl);
output_pcl = output1_pcl;
output_pcl += output2_pcl;
pcl::toROSMsg(output_pcl, output);
pub.publish(output);
}
void cloud_cb2(const sensor_msgs::PointCloud2ConstPtr& input2) {
listener->waitForTransform("/world_base_link", (*input2).header.frame_id, (*input2).header.stamp, ros::Duration(5.0));
sensor_msgs::PointCloud &pcout) const
pcl_ros::transformPointCloud("/world_base_link", *input2, output2, *listener);
pcl::fromROSMsg(output2, output2_pcl);
output_pcl = output2_pcl;
output_pcl += output1_pcl;
pcl::toROSMsg(output_pcl, output);
pub.publish(output);
}
int main (int argc, char **argv)
{
ros::init (argc, argv, "ar_kinect");
ros::NodeHandle n;
ARPublisher ar_kinect (n);
listener = new tf::TransformListener();
ros::Subscriber sub1 = n.subscribe("/overhead_kinect1/depth_registered/points", 1, cloud_cb1);
ros::Subscriber sub2 = n.subscribe("/overhead_kinect2/depth_registered/points", 1, cloud_cb2);
pub = n.advertise<sensor_msgs::PointCloud2>("/overhead_kinect_merged/depth_registered/points", 1);
ros::spin ();
return 0;
}
When I run one kinect alone I get data from /camera/deprth_registered/points, but when I merge - no response from /overhead_kinect_merged/depth_registered/points. Also the serial number of one kinect is zeros (which is rather weird), that's why I switched to bus parameters.
I am running out of ideas what could be the cause for it. I would be very thankful for any help or idea.
Originally posted by Andrii Matviienko on ROS Answers with karma: 56 on 2014-04-08
Post score: 0
Original comments
Comment by BennyRe on 2014-04-08:
Do the two Kinects actually work? I mean without the whole merging stuff. Do you get an image from both Kinects?
Comment by Andrii Matviienko on 2014-04-08:
Yes, both of them work absolutely fine separately
Comment by BennyRe on 2014-04-08:
Do you mean with separately, both together but without merging or each Kinect at a time?
Comment by Andrii Matviienko on 2014-04-08:
Both situations. If I run roslaunch openni_launch openni.launch for the 1st and then for the 2nd kinect - I get the data. When I run the lunch file specified above I get data from /overhead_kinect1 ( and 2) /depth_registered/points, but not from /overhead_kinect_merged/depth_registered/points
Comment by BennyRe on 2014-04-08:
Did you check that your callbacks get called properly?
Comment by Andrii Matviienko on 2014-04-08:
I don't know why, but when I changed 'depth_registered' to 'depth' it started giving me a merged point cloud. So it's solved, but why it did not work for 'depth_registered' is still a question.