Ask Your Question
0

TF Listener error when running two kinects with openni_launch

asked 2014-08-25 12:04:35 -0600

Damien gravatar image

updated 2014-08-25 12:05:48 -0600

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-08-25 21:43:54 -0600

Murilo F. M. gravatar image

I think the problem is your assumption that because listener is in the main, the buffer should have time to fill (assumptions are always a bit risky ;-). Another issue is that waitForTransform() will timeout upon failure and you are not checking whether it returned true or false.

Thus, I think you can solve your problem by replacing the waitForTransform() with:

if listener->canTransform(...) {
    pcl::transformPointCloud(...);
    pub.publish(output);
}
else {
    ROS_WARN("cannot transform from ... to ...");
}

If you really want to use the waitForTransform() and can afford to wait, do not pass a time as timeout.

If you want to use waitForTransform() with the timeout, make sure you check whether the timeout occurred and do not try to transform the point cloud and publish if it did timeout.

Regarding the way tf_monitor shows the chain, I have yet to understand that too. It appears it goes all the way up to the root, then traverses down to the desired frame, ignoring shorter paths.

edit flag offensive delete link more

Comments

Thanks.I was printing the bool in the console to check if the TF exists. The issue at the end is that with 1 kinect the code works nice and rotates correctly the pointcloud. But when I launch the second kinect node, NO TF is found... Might it come from the nodelets? Where should I start looking?

Damien gravatar imageDamien ( 2014-09-01 08:02:37 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-08-25 12:04:35 -0600

Seen: 360 times

Last updated: Aug 25 '14