Expose tool pose from RT connection in ROS

[sepjansen]

Solution for issue: #16

Features:

• Broadcast TF between base and tool as reported by UR controller. This is more accurate than the default pose from URDF model. Default base frame is 'base', tool frame is 'tool0_controller', both can be set using parameters
• Publish the tool velocity as TwistStamped with base frame as reference frame on topic 'tool_velocity'

Notes:

• This was tested in simulation and on real UR5 with version 3.0 and 3.1
• To use 'tool0_controller' as link in URDF, a link with that name should be connected to 'base' by a floating joint and the robot_state_publisher should have this fix: Check URDF to distinguish fixed joints from floating joints. Floating… ros/robot_state_publisher#32. This was tested and worked correctly.

[ThomasTimm]

Very nice work Simon!

Regarding your last note, am I understanding it correctly that this works with no problems with pre-indigo (the RSP fix is merged into indigo-devel), but the URDF with tool0_controller as a floating joint won't work, right?

[sepjansen]

The TF between base and tool frame is broadcasted correctly regardless of the fix in indigo-devel branch of robot_state_publisher, yes. You can view both tool_0 and tool0_controller links in RVIZ for your own robot and check the differences between the link poses using tf tooling. You should see a small difference (only on real robots, simulation should match perfectly).

However, to attach links in your URDF to tool0_controller (like gripper links), you need to have a link tool0_controller in your URDF. You basically need a 'dynamic' URDF link, where the actual transform is broadcast by the driver and not by the robot_state_publisher. You can do this by specifying a floating joint, which will only be correctly detected by robot_state_publisher since ros/robot_state_publisher#32.

You can of course always broadcast your own tf between tool0_controller using e.g. static transform broadcaster.

Btw: Do we know if that fix ros/robot_state_publisher#32 is already officially released? I tested by building robot_state_publisher from indigo-devel branch from source.

[ThomasTimm]

Great, thanks for clearing that out.

From http://docs.ros.org/indigo/changelogs/robot_state_publisher/changelog.html it looks like it is already in the official release, and thus a sudo apt-get install ros-indigo-robot-state-publisher should be sufficient.

[sepjansen]

Hmmm... sudo apt-get install ros-indigo-robot-state-publisher does not give me the new functionality, but that's probably another issue.