Merging 2 pcl from kinects

edit

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.