0
$\begingroup$

Rosanswers logo

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 ("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 10.2617 Hz, Average Delay: -0.0998126 Max Delay: 0
Node: /kinect1_base_link1 10.2613 Hz, Average Delay: -0.0998158 Max Delay: 0
Node: /kinect1_base_link2 10.2616 Hz, Average Delay: -0.0998104 Max Delay: 0
Node: /kinect1_base_link3 10.2616 Hz, Average Delay: -0.099813 Max Delay: 0
Node: /world_to_HRIworkspace 99.4637 Hz, Average Delay: -0.00981339 Max Delay: 0

Step4: I run the 2 kinects. -> The two poincloud are well seen in rviz. and the TF correclty applies the transformation in rviz.

-> I have the following error, EVEN if I don't publish the static TF for the second kinect. [ERROR] [1408986109.067499370]: "HRIworkspace" passed to lookupTransform argument target_frame does not exist.

-> The TF frames are correctly set: http://postimg.org/image/5ckc5r3tx/

-> tf_monitor shows:

RESULTS: for kinect1_rgb_optical_frame to HRIworkspace
Chain is: HRIworkspace -> world -> HRIworkspace -> kinect2_link -> kinect1_depth_frame -> kinect1_link -> kinect2_depth_frame -> kinect1_rgb_frame -> kinect1_link -> kinect2_link -> kinect2_rgb_frame
Net delay     avg = 0.00458208: max = 0.095311

Frames:

All Broadcasters:
Node: /HRIworkspace_to_kinect1_link 931.587 Hz, Average Delay: -0.0008456 Max Delay: 0.000991727
Node: /HRIworkspace_to_kinect2_link 931.519 Hz, Average Delay: -0.000847671 Max Delay: 0.000784096
Node: /kinect1_base_link 10.0039 Hz, Average Delay: -0.0998451 Max Delay: 0
Node: /kinect1_base_link1 10.0039 Hz, Average Delay: -0.0998217 Max Delay: 0
Node: /kinect1_base_link2 10.0039 Hz, Average Delay: -0.0998242 Max Delay: 0
Node: /kinect1_base_link3 10.0038 Hz, Average Delay: -0.0998209 Max Delay: 0
Node: /kinect2_base_link 10.004 Hz, Average Delay: -0.099828 Max Delay: 0
Node: /kinect2_base_link1 10.004 Hz, Average Delay: -0.0998313 Max Delay: 0
Node: /kinect2_base_link2 10.0038 Hz, Average Delay: -0.0998222 Max Delay: 0
Node: /kinect2_base_link3 10.0039 Hz, Average Delay: -0.099828 Max Delay: 0
Node: /world_to_HRIworkspace 99.2859 Hz, Average Delay: -0.00982809 Max Delay: 0

SO, my conclusions up to now:

  • The chain found by tf_monitor seems quite strange and not as direct as i would have thought.
  • The delay of doesn't seem to be an issue!... Does it??

I'm quite lost now, having looked at all the questions and tutorials I could find... Really need help here...

Any idea about how to solve this issue?

Anything to do with nodelets? (I don't control this aspect at all...)

Thanks in advance.

Damien


Originally posted by Damien on ROS Answers with karma: 203 on 2014-08-25

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Murilo F. M. with karma: 806 on 2014-08-25

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Damien on 2014-09-01:
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?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.