Rtabmap reference frame with ur5 and Kinect v2
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 ...
Just noticed this:
REVERSE_PORT
is not a valid value for thereverse_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.I've given you enough karma now.
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.
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!