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:
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
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.