TF Listener error when running two kinects with openni_launch
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 ...