Re: Setup Festo robot with Kinect2 for navigation

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Setup-Festo-robot-with-Kinect2-for-navigation-tp2552p2564.html

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