Setup Festo robot with Kinect2 for navigation

[marius]

Hi,

I`m new at ROS and try to navigate my Robot from company "Festo" with this great software and Microsoft Kinect2 (ONE).
The Robot is called "Robotino" and works with Ubuntu 16.04, ROS Kinetic.

The rtabmap compilation has succesfully finished and i have already tried the Hand-Held tutorial. http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping. It works amazingly!

Now i want to setup my Robot for navigation, but your tutorial (http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot) does not take me any further.

I found the rgbd_mapping_kinect2.launch file.
When i launch it the 3D scans (PointClouds) are visualized but the tranformations of /base_link /odom /kinect2_base_link does not match. When i move my robot around the map will be build and visualized in rviz but the /odom koordinate and some other seems to be fixed with the fixed frame /map.



The kinect2_base_link koordinates are visualized correctly.
I have tried to transform the geometric dependencis of base_link, odom and kinect2 by my self with static_transform_publisher but then the visualisation in rviz started to jitter :D

In the rtabmap_kinect2_launch file is only one TF between Kinect2_base_link and Kinect_link.
My robot has no gripper or some thing else, all is static there is only a odometry.

Can some one help me where/how i can setup up the tranformations between the different frames.

kind regards Marius

[marius]

ok,
new day and now the tf`s are working ^^

[matlabbe]

Hi,

For the record, the /kinect2_base_link should be linked to /base_link with a static_transform_publisher (or in your urdf if you are using robot state publisher) and set frame_id of rtabmap/rgbd_odometry to /base_link. If you are using odometry from your robot, don't forget to disable visual odometry if you use rtabmap.launch file (e.g., rgbd_odometry should not start), otherwise you will have two odometry publishing on the same frame /odom.

cheers,
Mathieu

[marius]

Thank you Mathieu for the response!

The manually tf between /base_link and /kinect2_base_link works.

Today i have tried to modify the rtabmap.launch file like u said.
I set the generel frame id to /base_link.
  <arg name="frame_id"                default="base_link"/> 

Then i disabled the visual_odometry.
  <arg name="visual_odometry"         default="false"/>   

Additionally i changed the camera topics for kinect2.
  <arg name="rgb_topic"               default="/kinect2/$(arg resolution)/image_color_rect" />
  <arg name="depth_topic"             default="/kinect2/$(arg resolution)/image_depth_rect" />
  <arg name="camera_info_topic"       default="/kinect2/$(arg resolution)/camera_info" />

When i launched the rtabmap.launch file the tf`s worked and the pointclouds are visualized but no map will be build.



When i launch the rgbd_mapping_kinect2.launch file the map will be built.
Furthermore there is a delay between the camera pictures and the visualization of my robot in rviz (~6 seconds).

I checked the tf tree when i only launched the kinect2_nodes and rtabmap without the static tf from /base_link to /kinect2_base_link. The /kinect2_base_link was connected with /odom. Maybe there i got problems?

When i activate the visual_odometry a map will be built. How can i do this with my own robot odometry?

Here is the rtabmap.launch that i modified. I do not understand any params.
file:///opt/ros/kinetic/share/rtabmap_ros/launch/rtabmap_robotino_setup.launch

I hope u can help me this will be great ;)

Marius

[matlabbe]

Hi,

In rgbd_mapping_kinect2.launch, there is a static transform between kinect2_base_link and kinect2_link:

   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
   <arg name="frame_id" default="kinect2_base_link"/>
   
   <!-- Rotate the camera -->
   <arg name="pi/2" value="1.5707963267948966"/>
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
        args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" /> 

In this setup you should indeed have a static transform publisher for /base_link -> /kinect2_base_link.

To use the general rtabmap.launch, "$(arg resolution)" should be defined. You can show the rqt_graph to see if the nodes are correctly connected, otherwise no map will be created.

Based on the Hand-Held Tutorial example, you can also start rtabmap like that:

$ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 kinect2_base_link kinect2_link 100

# Kinect is 10 cm over base_link
$ rosrun tf static_transform_publisher 0 0 0.1 0 0 0 base_link kinect2_base_link 100

$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rgb_topic:=/kinect2/qhd/image_color_rect depth_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info frame_id:="base_link" visual_odometry:=false odom_topic:="/odom"

or a launch file for convenience:

<launch>
   <arg name="pi/2" value="1.5707963267948966"/>

   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
        args="0 0 0 -$(arg pi/2) 0 -$(arg pi/2) kinect2_base_link kinect2_link 100" /> 

   <node pkg="tf" type="static_transform_publisher" name="base_to_kinect"
        args="0 0 0.1 0 0 0 base_link kinect2_base_link 100" /> 

   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
      <arg name="rtabmapviz"              value="false" />
      <arg name="rviz"                    value="true" /> 

      <arg name="rtabmap_args"          value="--delete_db_on_start" /> 
      <arg name="rgb_topic"             value="/kinect2/qhd/image_color_rect" /> 
      <arg name="depth_topic"           value="/kinect2/qhd/image_depth_rect" /> 
      <arg name="camera_info_topic"     value="/kinect2/qhd/camera_info" /> 
      <arg name="frame_id"              value="base_link" />  
      <arg name="visual_odometry"       value="false" /> 
      <arg name="odom_topic"            value="/odom" /> 
   </include>
</launch>

Does your robot publish odometry topic? The example above assumes that /odom topic is published ("$rostopic hz /odom").

cheers,
Mathieu

[marius]

Thanks Matheu!

I will try it today.

Yes my robot publishs the topic /odom. I`m working with this robot: http://www.festo-didactic.com/de-de/lernsysteme/robotino-forschen-und-lernen-mit-robotern/robotino-fuer-forschung-und-bildung-premium-edition-und-basic-edition.htm?fbid=ZGUuZGUuNTQ0LjEzLjE4Ljg1OC44MDI1 and a ROS package with the basic node http://wiki.ros.org/robotino_node

marius

[marius]

Ok,
I`ve tried the method without editing the rtabmap.launch. The static tf`s and the roslaunch in single shells.

Last week i have already used this two static tf`s between base_link->kinect2_base_link->kinect2_link but i checked it again.
He is the tf tree: I only run my odometry_node (robotino_node) and the kinect2_node (aia_kinect2) and this two tf`s.


When i roslaunch your command i can visualize my pointclouds and also see my tf`s are moving, but no map will be built!
The tf`s are running in rviz, but there are some strange effects. There is a large delay between the pointclouds (nearly live) and the shown tf`s (delay ~6 seconds). When i dont use the static_tf between base_link and the camera the odom tf`s (base_link and wheel frames ...) moving simultaneously in rviz. But i think that is not the problem the no map appear...

Here is a part of my node graph.



I have already used your roslaunch parameters: only the rtabmap_args i added today.

      <arg name="rtabmap_args"          value="--delete_db_on_start" /> 
      <arg name="rgb_topic"             value="/kinect2/qhd/image_color_rect" /> 
      <arg name="depth_topic"           value="/kinect2/qhd/image_depth_rect" /> 
      <arg name="camera_info_topic"     value="/kinect2/qhd/camera_info" /> 
      <arg name="frame_id"              value="base_link" /> 
      <arg name="visual_odometry"       value="false" /> 
      <arg name="odom_topic"            value="/odom" /> 

My next post is the eddited rtamp.launch i`ve try to use. Maybe u can check this.

[marius]

RTABMAP_ROBOTINO_SETUP

<launch>
 
  <arg name="resolution" default="qhd" />
       
  <arg name="stereo"          default="false"/>
 
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
 
  <arg name="localization"            default="false"/>
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
  <arg name="frame_id"                default="base_link"/>     
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="rtabmap_args"            default="delete_db_on_start"/>               
  <arg name="launch_prefix"           default=""/>             
  <arg name="approx_sync"             default="true"/>         
  <arg name="rgb_topic"               default="/kinect2/$(arg resolution)/image_color_rect" />
  <arg name="depth_topic"             default="/kinect2/$(arg resolution)/image_depth_rect" />
  <arg name="camera_info_topic"       default="/kinect2/$(arg resolution)/camera_info" />
 
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />     
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  <arg name="compressed"              default="false"/>         
                                                                 
                                                               
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>

  <arg name="visual_odometry"         default="false"/>         
  <arg name="odom_topic"              default="/odom"/>         
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
 
 
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>

 
  <group ns="$(arg namespace)">
 
   
    <group unless="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
 
   <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
        <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       
       
       
       
       
       
      </node>


    </group>
   

   
   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">

     
     
     
     
     
     
     
     
     
     
     
     
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
     
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
     
     
         
         
         
    </node>
 
   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
   
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
     
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
     
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
 
  </group>
 
 

  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>


  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>


  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">

    <remap from="rgb/image_in"       to="$(arg rgb_topic_relay)"/>
    <remap from="depth/image_in"     to="$(arg depth_topic_relay)"/>
    <remap from="rgb/camera_info_in" to="$(arg camera_info_topic)"/>

    <remap from="odom_in"             to="$(arg odom_topic)"/>

   
   
    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
    <remap from="depth/image_out"     to="data_odom_sync/depth"/>
    <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
    <remap from="odom_out"            to="odom_sync"/>
  </node>


  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
    <remap from="rgb/image"       to="data_odom_sync/image"/>
    <remap from="depth/image"     to="data_odom_sync/depth"/>
    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />

   
  </node>






</launch>

[matlabbe]

Hi,

Is the robot TF published from a different computer? Looking at your TF tree, some transforms has 10 sec behind others. If there are multiple computers, make sure their clocks are synchronized. See for example : http://answers.ros.org/question/223684/rtabmap-not-working-with-kinectodometry/

gvdhoorn wrote

 if possible, just synchronise the time of the involved PCs. Use something like chrony or ntp. No need to write custom nodes.

cheers,
Mathieu

[marius]

Yes that`s right

The kinect2_node and rtabmap_node is running on my laptop. The robot has its own computer where the /odom and some other topics are published. The pcs communicate with WLAN. Yesterday i tried to sync the time stamps of this two computers with ntp and chrony but i does`t worked. The ntpdate or chronyd daemon is not available but i asked about this in the ubuntusers forum how to fix it. Is there an other way to sync this time stamps?

According two your answer that the time stamps are not syncronized: you talk about the buffer length ord oldest transform in the tf-tree? where i can see that the time stamps are not sync?

Today i studied the node who handle the odometry of the robot called robotino_node (ros.org)
I commented some nodes out that i only have the odom and base_link tf like this. (I shot this pic on my master laptop and the node is running on the robot computer)



Step 2: run iaia_kinect2_node on master laptop



Step 3: tf between base_link -> Kinect2_base_link -> kinect2_link



That`s the tf when i only run the rgbd_kinect2_mapping_node on my laptop with visual odometry.

[matlabbe]

Hi,

I was refering to "Most recent transform" in the TF tree. We can see that /odom -> /base_link is always in advance to laptop time (here ~4.5 sec). You may have to install a ntp server on the robot, or do ntp from the robot to your laptop.

cheers