Ask Your Question
0

Rtabmap reference frame with ur5 and Kinect v2

asked 2017-09-12 07:40:49 -0500

budekatude gravatar image

updated 2017-09-12 08:33:35 -0500

gvdhoorn gravatar image

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)

edit retag flag offensive close merge delete

Comments

<param name ="reverse_port" value ="REVERSE_PORT"/>

Just noticed this: REVERSE_PORT is not a valid value for the reverse_port parameter. If you don't need it (ie: you have only a single robot and don't have a restrictive firewall), don't specify that parameter. It will use a default.

gvdhoorn gravatar imagegvdhoorn ( 2017-09-12 08:32:33 -0500 )edit

Unfortunately I'm missing Karma to attach the tf view_frames output.

I've given you enough karma now.

gvdhoorn gravatar imagegvdhoorn ( 2017-09-12 08:34:12 -0500 )edit

Is the arm fixed on a mobile base or a static base? What is the base frame of the platform? You would have to set frame_id of rgbd_mapping_kinect2.launch to this frame, otherwise the map is built against kinect frame, not the base frame.

matlabbe gravatar imagematlabbe ( 2017-09-12 11:01:40 -0500 )edit

Thanks for all the input (and the Karma gvdhoorn :D )! Turns out matlabbes answer took the price and it's working perfectly now!! @matlabbe: if you want to post your solution as an answer I'll be sure to mark it as the best solution! Thanks again everyone for your input! Very much appreciated!

budekatude gravatar imagebudekatude ( 2017-09-13 09:46:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-09-13 12:26:50 -0500

matlabbe gravatar image

(Here is the copy of my comment above for convenience)

What is the base frame of the platform? You would have to set frame_id of rgbd_mapping_kinect2.launch to this frame, otherwise the map is built against kinect frame, not the base frame.

For example, if the base frame of the robot is base_footprint, the launch above would look like this:

<include file ="$(find rtabmap_ros)/launch/rgbd_mapping_kinect2.launch">
    <arg name ="resolution" value="qhd"/>
    <arg name="frame_id" value="base_footprint"/>
</include>

cheers

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2017-09-12 07:40:49 -0500

Seen: 238 times

Last updated: Sep 13 '17