Cartographer SLAM - TF Tree Errors

asked 2019-02-26 10:49:51 -0600

JanOr
170 ●24 ●28 ●33

updated 2019-02-28 04:01:22 -0600

Hi everyone,

I am completely stuck on a tf problem. What I try to do is slam with a laser sensor and an imu with cartographer_ros. According to the related tutorial, I define my physical layout of the robot in a urdf file:

<robot name="robot_3d">
<material name="orange"> <color rgba="1.0 0.5 0.2 1"/> </material>
<material name="gray"> <color rgba="0.2 0.2 0.2 1"/> </material>

<link name="imu_link"> <visual> <origin xyz="0.0 0.0 0.13"/> <geometry> <box size="0.06 0.04 0.02"/> </geometry> <material name="orange"/> </visual> </link>

<link name="base_laser_link"> <visual> <origin xyz="0.0 0.0 0.035"/> <geometry> <cylinder length="0.15" radius="0.08"/> </geometry> <material name="gray"/> </visual> </link>

<link name="base_link"/>

<joint name="imu_link_joint" type="fixed"> <parent link="base_link"/> <child link="imu_link"/> <origin xyz="0 0 0" rpy="0 0 0"/> </joint>

<joint name="base_laser_link_joint" type="fixed"> <parent link="base_link"/> <child link="base_laser_link"/> <origin xyz="0.0 0. 0.0" rpy="0 0 0"/> </joint>

</robot>

This file clearly defines the transformation between base_link and imu_link as well as base_link with base_laser_link. Than I call cartographer to do slam which is calling state_publisher. However, cartographer shows an error that it cannot relate the imu_link to the base_link. Also in tf viewer in rqt it is visible, that imu_link and base_link are not connected via a transformation, while base_link and base_laser_link are connected without problems. Did anyone of you have a similar issue? Why doe the state_publisher not relate the imu_link with the base_link?

* Update *

The TF tree appears to be constructed correctly just using the robot state publisher. This means the base_link is parent to imu_link and base_laser_link. However, as soon as I start cartographer_ros node the tf tree is ripped apart. Then it shows the new topic odom (which shall be generated by the slam) which is parent to imu_link. Separatly (unconnected) to this also we still have the base_link topic which is parent to "base_laser_link". The error the cartographer is throwing is:

"Could not find a connection between 'imu_link' and 'base_laser_link' because they are not part of the same tree.Tf has two or more unconnected trees.

Before starting cartographer After starting cartographer:

* Update * I could further break down the error, that the imu manager is causing this tf fault. The imu is based one the phidget imu package:

sudo apt install ros-melodic-phidgets-imu

Which is called with the following launch file:

<launch>
  <node pkg="nodelet" type="nodelet" name="imu_manager" 
    args="manager" output="screen" />

  <node pkg="nodelet" type="nodelet" name="PhidgetsImuNodelet" 
    args="load phidgets_imu/PhidgetsImuNodelet imu_manager" 
    output="screen">
    <param name="period" value="4"/>
    <param name="frame_id" value="imu_link"/> 
  </node>

  <node pkg="nodelet" type="nodelet" name="ImuFilterNodelet" 
    args="load imu_filter_madgwick/ImuFilterNodelet imu_manager" 
    output="screen">

    <param name="use_mag" value="true"/>
    <param name="use_magnetic_field_msg" value="true"/>
  </node>

</launch>

However, I cannot find out why this imu manager is creating a tf connection between frame /odom and /imu_link and at the same time breaking /imu_link and ... (more)

answered 2019-02-26 15:37:50 -0600

Delb

3907 ●16 ●105 ●68

updated 2019-02-27 10:17:20 -0600

<material> name="orange"> <color rgba="1.0 0.5 0.2 1"/> </material> ^ You have a typo here inducing that your material isn't correctly defined. So when you use the material orange for the link imu_link you get an error and the link isn't created.

Note : When you have something wrong with an URDF file you can use tools like check_urdf that will tell you every mistakes/errors/typos.

Edit : The frame odom is actually published by your nodelet ImuFilterNodelet. From the imu_filter_madgwick wiki :

~fixed_frame (string, default: odom)
The parent frame to be used in publish_tf.
~publish_tf (bool, default: true)
Whether to publish a TF transform that represents the orientation of the IMU, using the frame specified in fixed_frame as the parent frame and the frame given in the input imu message as the child frame.

Since you don't set those two parameters they are set to their default value, meaning that a frame odom is published and is set as the parent of the frame related to your imu messages (i.e. imu_link as defined when calling your other nodelet PhidgetsImuNodelet). So you end up with imu_link having odom as a parent from your nodelet AND base_link as a parent from the node robot_state_publisher, which explains why your tf tree is broken.

That being said, I'm not entirely sure about what you should do. You should see this discussion.

You'll probably have to change your URDF to set imu_link as the parent of base_link instead of its child and then change the tracking_frame param from cartographer to the base_link (you shouldn't have removed the cartographer params from your previous update, they are part of your issue. Can you add them back please ?).

I hope that will help you.

edit flag offensive delete link

Comments

Hi, thanks. Sorry this is a copy paste error. I updated the code above, but unfortunately this does not resolve the issue.

JanOr ( 2019-02-27 01:47:40 -0600 )edit

What is the issue ? I just used your urdf and check_urdf returns no errors and the robot is correctly displayed in Rviz with all the frames connected.

Delb ( 2019-02-27 01:55:32 -0600 )edit

Hi, thanks you are completely right. I have updated the question/error accordingly.

JanOr ( 2019-02-27 02:33:15 -0600 )edit

Thanks for the update, can you add a screenshot of your tf tree please ?

Delb ( 2019-02-27 02:48:31 -0600 )edit

Hi, you can find the screenshots above.

JanOr ( 2019-02-27 03:28:29 -0600 )edit

What is this /imu_manager broadcaster ? Is it the name you used when launching the cartographer node ? Also, since your frames base_imu and base_link are at the same pose and fixed, what happen if you set the param tracking_frame to base_link ?

Delb ( 2019-02-27 04:05:40 -0600 )edit

Hi, thanks. Actually I thought the imu manager is called by the cartographer, however this is not true. I get the same problem if I only run the imu. I have updated the question...

JanOr ( 2019-02-27 09:14:55 -0600 )edit

The velocity filter of the imu package has been using a tf tree for the filtering. By only using the raw imu data the problem has been resolved. The cartographer map still doesn't work, I opened a new question for this. Thanks a lot for your help!

JanOr ( 2019-02-28 04:02:17 -0600 )edit

add a comment

Cartographer SLAM - TF Tree Errors

1 Answer

Comments

Question Tools

Stats

Related questions

Cartographer SLAM - TF Tree Errors edit

1 Answer

Comments

Question Tools

Stats

Related questions

Cartographer SLAM - TF Tree Errors