Octomap doesn't get created

[AlbertoGhiotto]

Hi!

I'm trying to create a 3D octomap using a kinect camera mounted on a simple ground robot to then use if for path planning with the Kuka manipulator. I setup a gazebo simulation with some simple objects to be inserted in the map. I inserted the ground robot with the kinect mounted on it. The robot is moved with a modified version of the teleop_twist_keyboard script.

To launch rtabmap I'm using the rtabmap.launch launch file in which I modified the RGB-D related topic to read from the ones published by my kinect as follows:

<arg name="rgb_topic"               default="/camera/color/image_raw" /> 
<arg name="depth_topic"             default="/camera/depth/image_raw" /> 
<arg name="camera_info_topic"       default="/camera/color/camera_info" /> 
<arg name="depth_camera_info_topic" default="/camera/depth/camera_info" />

I'm a bit confused on how I need to setup the frames to have the node work correctly. Nevertheless, since I'd want to use TF as source for odometry, I setup the frames as follows:

<arg name="frame_id"                default="panda_link0"/>      
<arg name="odom_frame_id"           default="chassis"/>                
<arg name="map_frame_id"            default="panda_link0"/>

Where chassis is the frame of the ground robot in gazebo and panda_link0 is the frame of the base of the Panda spawned in Rviz with the perception_pipeline moveit tutorial.

Since I understand that I need an optical rotation to align the frames I've put in the Rviz launch file the following:

<node pkg="tf2_ros" type="static_transform_publisher" name="panda_chassis" args="0 0 0.4 0 3.07 1.57 panda_link0 chassis" />
<node pkg="tf" type="static_transform_publisher" name="camera_optical_rotation" args="0 0 0.4 0 3.07 1.57 odom panda_link0 200" />

I'm using both tf and tf2 to try with the different packages.

Now, the problem is that as the ground robot and the kinect move, the map doesn't get created as I think it should but it is all over the place, as you can see from the following images. I'm visualizing the topic /rtabmap/octomap_full in Rviz.

I would like the octomap to be created as the 3D model of the gazebo simulation to then use it as the planning scene for the Panda manipulator.

Here you can see my frame tree: frames.pdf

Here you can find my rtabmap db.

I would really appreciate any help! Thanks a lot!

[matlabbe]

Is the panda manipulator on the mobile robot? If so, the TF tree is wrong. Your robot should have an odometry plugin that will give its odometry (odom -> chassis). The relation between chassis and panda_link0 (the base of the arm) should be chassis -> panda_link0. rtabmap would be configured with:

<arg name="frame_id"                default="chassis"/>      
<arg name="odom_frame_id"           default="odom"/>                
<arg name="map_frame_id"            default="map"/>

The final TF tree will look more like map -> odom -> chassis -> panda_link0 -> panda_link1 ... Update your URDF to connect chassis and panda_link0. robot_state_publisher will then publish chassis->panda_link0. For odom->chassis, it will be your plugin used to move the robot (e.g., Differential drive plugin). Then map->odom will be published by rtabmap.

cheers, Mathieu

[AlbertoGhiotto]

Thanks for the reply!

The panda manipulator is fixed on the ground. The final version of my simulation will have the camera placed on the end effector in order to have it map the area in real time while doing path planning. This is a demo to try to make things works.

My robot uses the Planar Move Plugin to move and compute its odometry.

I tried to implement your instructions, I setup the frames as you said:

<arg name="frame_id"                default="chassis"/>      
<arg name="odom_frame_id"           default="odom"/>                
<arg name="map_frame_id"            default="map"/>

and published the relation between chassis and panda_link0 as:

<node pkg="tf2_ros" type="static_transform_publisher" name="chassis_panda" args="0 0 0.4 0 0 0 chassis panda_link0" />

Then final TF tree looks like you said: frames.pdf

However, the map is still displayed as before in different places instead of being updated as the robot moves in the gazebo simulation:

Moreover, Rviz shows the error "Failed to transform from frame [map] to frame [panda_link0]."

What am I missing here?

Thanks!

[matlabbe]

I don't see the arm in the simulator. You cannot use odomety from one robot, and apply it to another fixed robot (the arm). Just adding the static transform publisher between the mobile base and the fixed arm won't link them in the simulator. You should modify the URDF to include both your mobile base and the arm in the same file, linked together with a joint. I would expect to see the arm on the mobile base in the simulator.