TF Listener error when running two kinects with openni_launch

edit

Hi.

I'm trying to achieve something simple:

• launch 2 kinects
• publish static TF between kinects and HRIworkspace frames
• merge the 2 pointclouds in HRIworkspace
• run octomaps for later moveit planning. This is the fundamental issue: Octomaps cannot take as input multiple Pointcloud: it works only with 1 pointcloud. So I need to rotate and merge my individual pointcloud so that i can fill the result into octomap...

My issue is that I have it running with one kinect. but "just" running a second node of kinect breaks something and listener->waitForTransform throws me the typical error: [ERROR] [1408981523.817028455]: "HRIworkspace" passed to lookupTransform argument target_frame does not exist.

Import thing: I know that I can have only 1 Kinect per Hub. And I did pay attention to this.

so, Step by step:

Step 1. Kinect Launch files. Launches both kinects and the static publishers for the kinects links.

<!-- Parameters possible to change-->
<arg name="camera1_id" default="1@0" />
<arg name="camera2_id" default="2@0" />
<arg name="depth_registration" default="true" />

<!-- Default parameters-->
<arg name="camera1_name" default="kinect1" />
<arg name="camera2_name" default="kinect2" />

<node pkg="tf" type="static_transform_publisher" name="world_to_HRIworkspace" args="0 0 0 0 0 0 /world /HRIworkspace 10" /> 

<!-- Launching first kinect-->
<include file="$(find openni_launch)/launch/openni.launch">
  <arg name="device_id" value="$(arg camera1_id)" />
  <arg name="camera" value="$(arg camera1_name)" />
  <arg name="depth_registration" value="$(arg depth_registration)" />
  <arg name="publish_tf" value="true"/>
</include>

<node pkg="tf" type="static_transform_publisher" name="HRIworkspace_to_kinect1_link"  args="2 -2 2 2.356 0.35 0 /HRIworkspace /kinect1_link 1" /> 

<!-- Launching second kinect-->
<include file="$(find openni_launch)/launch/openni.launch">
  <arg name="device_id" value="$(arg camera2_id)" />
  <arg name="camera" value="$(arg camera2_name)" />
  <arg name="depth_registration" value="$(arg depth_registration)" />
  <arg name="publish_tf" value="true"/>
</include>

<node pkg="tf" type="static_transform_publisher" name="HRIworkspace_to_kinect2_link" args="2 2 2 -2.356 0.35 0 /HRIworkspace /kinect2_link 1" />

Step 2. The future merging node, with the tf listner.

Ok, for debug purposes, it doesn't do much of merging... but I need this to run before implementing more. Now it only tries to get the transform and transform the pointcloud...

Note that the definition of the tf: listener is the main, so its buffer should have time to fill...

void cloud_cb1(const sensor_msgs::PointCloud2ConstPtr& input1) {
       listener->waitForTransform("kinect1_rgb_optical_frame","HRIworkspace", (*input1).header.stamp, ros::Duration(10.0) );
       pcl_ros::transformPointCloud("HRIworkspace",*input1, output1, *listener);
       pub.publish(output);    
    }

    int main (int argc, char** argv)
    {
    // Initialize ROS
    ros::init (argc, argv, "merge2pcl2");
    ros::NodeHandle nh;

    listener = new tf::TransformListener();
    ros::Subscriber sub1 = nh.subscribe ("/kinect1/depth_registered/points", 1, cloud_cb1);
    pub = nh.advertise<sensor_msgs::pointcloud2> ("output", 1);
    ros::spin ();
    }

Step3: If i run only 1 kinect:

No problem all TF transforms are found, code running smoothly, republishing the pointcloud. TF echo runs ok rosrun tf tf_monitor kinect1_rgb_optical_frame HRIworkspace runs ok

RESULTS: for kinect1_rgb_optical_frame to HRIworkspace
Chain is: HRIworkspace -> world -> kinect1_link -> kinect1_rgb_frame -> kinect1_link -> kinect1_depth_frame
Net delay     avg = -0.000275143: max = 0.000302901

Frames:

All Broadcasters:
Node: /HRIworkspace_to_kinect1_link 930.502 Hz, Average Delay: -0.000832957 Max Delay: 0
Node: /kinect1_base_link ...