Rtabmap reference frame with ur5 and Kinect v2

edit

Hi all,

I've been trying to create a collision map of the surroundings (with rtabmap) for a Universal Robot ur5 using a Microsoft Kinect v2 mounted to an elbow close to the end effector of the robot. I am using ROS Indigo and Ubuntu 14.04.

I have written a .launch file that starts up all the nodes. Most of the elements of the algorithm are working fine, except that I don't seem to get the reference frame for the map right.... What do I need to link the map frame to in order to get the mapping in reference to the robot right even when it's moving (I've written a motion_publisher node that makes the robot follow a trajectory for looking around so as to map the whole area around it).

This is the .launch file I'm using:

<launch>

<!--By default , the database db is not started -->
    <arg name ="db" default ="false"/>

<!--By default , debug mode is switched off -->
    <arg name ="debug" default ="false" />

<!-- Load the URDF , SRDF and other . yaml configuration files on the param server -->
    <include file ="$(find ur5_moveit_config)/launch/planning_context.launch">
        <arg name ="load_robot_description" value ="true"/>
    </include>

<!--Including Kinect Bridge camera driver-->
    <include file ="$(find kinect2_bridge)/launch/kinect2_bridge.launch">
    <arg name ="publish_tf" value="true"/>
    </include>

<!--static_transform_publisher (transformation description) frame_id child_frame_id period_in_ms-->
    <!-- Link kinect2_base_link frame to ee_link -->
    <node pkg ="tf"
    type ="static_transform_publisher"
    name ="kinect2_base_link_tf_broadcaster"
        args ="-0.09 0 0.07 0 0 0 ee_link kinect2_base_link 10"/>

    <!-- Link map frame to ee_link -->
    <node pkg ="tf"
    type ="static_transform_publisher"
    name ="map_tf_broadcaster"
        args ="-0.09 0 0.07 0 0 0 ee_link map 10"/>

<!-- Run UR5 drivers with ip of plugged in robot to connect to it-->
   <include file ="$(find ur_modern_driver)/launch/ur5_bringup.launch">
        <arg name ="robot_ip" value ="192.168.100.200"/>
        <!--<arg name ="limited" value ="true" />-->
        <param name ="reverse_port" value ="REVERSE_PORT"/>
    </include>

<!--Run the MoveIt config for the ur5-->
        <include file ="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
            <arg name ="limited" value ="false"/>
        </include>

<!--Run Rviz and load the saved config to see the state of the move_group node-->
    <node pkg="rviz" 
    type="rviz" 
    name="my_rviz" 
    args="-d $(find MotionPublisher)/launch/rvizconfig_qhdkinect.rviz"/>           

            <!--Starting up rtabmap in mapping mode-->
    <include file ="$(find rtabmap_ros)/launch/rgbd_mapping_kinect2.launch">
    <arg name ="resolution" value="qhd"/>
    </include>

<!--Start Motion Publisher to publish trajectory points for mapping-->
    <node name ="MotionPublisher_node" 
        pkg ="MotionPublisher" 
        type ="MotionPublisher_node"/>
</launch>

With the current configuration (as shown in the .launch file above) the mapping seems to work fine for a while, but after some movements the map seems to loose track... objects are appearing multiple times at different angles from the robot and the map usually shifts to a weird angle from the robot (mostly some sort of an inclined plane to the robots base plane).

To me it looks like the transformations aren't calculated correctly. Sure enough I do get "[ERROR] Found empty JointState message" every now and then... not sure where it's coming from and if there's a connection!

I am not entirely sure ... (more)