URDF Configuration - Code Walkthrough MoreView sourceHistoryPage

Introduction

This program demonstrates the usage how to construct a URDF Configuration message in Python, to be sent into Baxter's Mutable Robot State Publisher (MRSP).
The MRSP will then update the onboard URDF to add your desired new Joint-Link tree fragment. You can view a Xacro that can be used to create such a segment tree on GitHub:
https://github.com/RethinkRobotics/baxter_common/blob/master/baxter_description/urdf/left_end_effector.urdf.xacro

Code Walkthrough

Now, let's break down the code.

30 import os
31 import sys
32 import argparse
33 
34 import rospy
35 import xacro_jade
36 
37 from baxter_core_msgs.msg import (
38     URDFConfiguration,
39 )

The standard Python libraries of os, sys, argparse are imported here. We also import xacro_jade to utilize some of the wonderful new features that have been backported to Xacro ROS Indigo from Xacro ROS Jade python API's. See https://github.com/ros/xacro/pull/117 for more details on Xacro Jade backporting.
As for the actual message itself, we need to import the URDFConfiguration message for constructing the message to notify the Mutable Robot State Publisher that a URDF change has occurred.

41 def xacro_parse(filename):
42     doc = xacro_jade.parse(None, filename)
43     xacro_jade.process_doc(doc, in_order=True)
44     return doc.toprettyxml(indent='  ')

Here, we are using the xacro_jade in_order functionality, to ensure our URDF is constructed in the order the Xacro files are included.

46 def send_urdf(parent_link, root_joint, urdf_filename):
47     """
48     Send the URDF Fragment located at the specified path to
49     modify the electric gripper on Baxter.
50     @param parent_link: parent link to attach the URDF fragment to
51                         (usually <side>_hand)
52     @param root_joint: root link of the URDF fragment (usually <side>_gripper_base)
53     @param urdf_filename: path to the urdf XML file to load into xacro and send
54     """
55     msg = URDFConfiguration()
56     # The updating the time parameter tells
57     # the robot that this is a new configuration.
58     # Only update the time when an updated internal
59     # model is required. Do not continuously update
60     # the time parameter.
61     msg.time = rospy.Time.now()
62     # link to attach this urdf to onboard the robot
63     msg.link = parent_link
64     # root linkage in your URDF Fragment
65     msg.joint = root_joint
66     msg.urdf = xacro_parse(urdf_filename)
67     pub = rospy.Publisher('/robot/urdf', URDFConfiguration, queue_size=10)
68     rate = rospy.Rate(5) # 5hz
69     while not rospy.is_shutdown():
70         # Only one publish is necessary, but here we
71         # will continue to publish until ctrl+c is invoked
72         pub.publish(msg)
73         rate.sleep()

Here the URDFConfiguration message is constructed. It's composed of four parts:

• time: Time at which URDF was updated.
  This is time is used to notify the robot that a change has taken place. You if you publish several messages at the same time using the same link and joint, no update will occur. If the time is incremented (as clock time tends to do by itself :), the Mutable Robot State Publisher will continually update the robot (quite a bad thing). So simply update the time field only when the URDF fragment has changed.
• link: Link that is in the already existing URDF in the robot's robot_description parameter which you would like to attach your fragment to.
• joint: The joint's name inside of your URDF fragment which should be used to attach your fragment to the specified link field above.
• urdf: The URDF Fragment Tree where you can describe your new end effector or camera.
  Note: If you have any moving (revolute, prismatic) joints in this fragment, you must separately publish joint states for them on the /robot/joint_states topic.

Here we publish the message at 5 Hz, taking special care not to update the time field, which would trigger continuous updates, since only one change is desired.

 76 def main():
 77     """RSDK URDF Fragment Example:
 78     This example shows a proof of concept for
 79     adding your URDF fragment to the robot's
 80     onboard URDF (which is currently in use).
 81     """
 82     arg_fmt = argparse.RawDescriptionHelpFormatter
 83     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
 84                                      description=main.__doc__)
 85     required = parser.add_argument_group('required arguments')
 86     required.add_argument(
 87         '-f', '--file', metavar='PATH', required=True,
 88         help='Path to URDF file to send'
 89     )
 90     required.add_argument(
 91         '-l', '--link', required=False, default="left_hand",
 92         help='URDF Link already to attach fragment to (usually <left/right>_hand)'
 93     )
 94     required.add_argument(
 95         '-j', '--joint', required=False, default="left_gripper_base",
 96         help='Root joint for fragment (usually <left/right>_gripper_base)'
 97     )
 98     args = parser.parse_args(rospy.myargv()[1:])
 99 
100     rospy.init_node('rsdk_configure_urdf', anonymous=True)
101 
102     if not os.access(args.file, os.R_OK):
103         rospy.logerr("Cannot read file at '%s'" % (args.file,))
104         return 1
105     send_urdf(args.link, args.joint, args.file)
106     return 0

This function simply takes in the parameters, substitutes defaults when necessary, reads the desired Xacro file, and calls the send_urdf function with those parameters.

108 if __name__ == '__main__':
109     sys.exit(main())

Typical python main function called when this file is executed as a script.