def test_include_glob(self):
doc = parseString('''<a xmlns:xacro="http://www.ros.org/xacro">
<xacro:include filename="include*.xml" /></a>''')
xacro.process_includes(doc,
os.path.dirname(os.path.realpath(__file__)))
self.assertTrue(
xml_matches(
doc,
'<a xmlns:xacro="http://www.ros.org/xacro"><foo /><bar /><baz /></a>'
))
def __init__(self, timeline_frame):
QWidget.__init__(self)
self.timeline_frame = timeline_frame
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bag_annotation'), 'resource', 'annotation_widget.ui')
loadUi(ui_file, self)
self.setObjectName('AnnotationWidget')
rd_file = os.path.join(rp.get_path('turtlebot_description'), 'robots', 'kobuki_hexagons_kinect.urdf.xacro')
a = xacro.parse(rd_file)
xacro.process_includes(a, rd_file)
xacro.eval_self_contained(a)
rospy.set_param('robot_description', a.toxml())
self._cur_annotation = None
self.save_button.clicked[bool].connect(self.save)
def check_name(self, hand_urdf_path):
with open(hand_urdf_path, 'r') as hand_urdf_xacro_file:
hand_urdf_xml = parse(hand_urdf_xacro_file)
xacro.process_includes(hand_urdf_xml, os.path.dirname(sys.argv[0]))
xacro.eval_self_contained(hand_urdf_xml)
hand_urdf = hand_urdf_xml.toprettyxml(indent=' ')
robot = URDF.from_xml_string(hand_urdf)
is_biotac = False
# Check if hand has the old biotac sensors
for key in robot.link_map:
link = robot.link_map[key]
if link.visual:
if hasattr(link.visual.geometry, 'filename'):
filename = os.path.basename(link.visual.geometry.filename)
if filename == "biotac_decimated.dae":
is_biotac = True
break
return is_biotac
def check_name(self, hand_urdf_path):
with open(hand_urdf_path, 'r') as hand_urdf_xacro_file:
hand_urdf_xml = parse(hand_urdf_xacro_file)
xacro.process_includes(hand_urdf_xml, os.path.dirname(sys.argv[0]))
macros = {}
xacro.grab_macros(hand_urdf_xml, macros)
symbols = xacro.Table()
xacro.grab_properties(hand_urdf_xml, symbols)
xacro.eval_all(hand_urdf_xml.documentElement, macros, symbols)
hand_urdf = hand_urdf_xml.toprettyxml(indent=' ')
robot = URDF.from_xml_string(hand_urdf)
is_biotac = False
# Check if hand has the old biotac sensors
for key in robot.link_map:
link = robot.link_map[key]
if link.visual:
if hasattr(link.visual.geometry, 'filename'):
filename = os.path.basename(link.visual.geometry.filename)
if filename == "biotac_decimated.dae":
is_biotac = True
break
return is_biotac
def test_include_glob(self):
doc = parseString('''<a xmlns:xacro="http://www.ros.org/xacro">
<xacro:include filename="include*.xml" /></a>''')
xacro.process_includes(doc, os.path.dirname(os.path.realpath(__file__)))
self.assertTrue(
xml_matches(doc, '<a xmlns:xacro="http://www.ros.org/xacro"><foo /><bar /><baz /></a>'))
def __init__(self, description_file=None, load=True):
if description_file is None and len(sys.argv) > 1:
description_file = sys.argv[1]
self._save_files = rospy.get_param('~save_files', False)
self._path_to_save_files = rospy.get_param('~path_to_save_files',
"/tmp/")
self._file_name = rospy.get_param('~file_name', "generated_robot")
# ARM
self.rospack = rospkg.RosPack()
self.package_path = self.rospack.get_path('sr_multi_moveit_config')
self.robot = Robot()
with open(description_file, "r") as stream:
yamldoc = yaml.load(stream)
self.robot.set_parameters(yamldoc)
new_srdf_file_name = "generated_robot.srdf"
self.start_new_srdf(new_srdf_file_name)
for manipulator in self.robot.manipulators:
if manipulator.has_arm:
# Read arm srdf
arm_srdf_path = manipulator.arm.moveit_path + "/" + manipulator.arm.name + ".srdf"
with open(arm_srdf_path, 'r') as stream:
self.arm_srdf_xml = parse(stream)
xacro.process_includes(self.arm_srdf_xml,
os.path.dirname(sys.argv[0]))
xacro.process_doc(self.arm_srdf_xml)
if manipulator.has_hand:
# Generate and read hand srdf
hand_urdf_path = self.rospack.get_path(
'sr_description') + "/robots/" + manipulator.hand.name
with open(hand_urdf_path, 'r') as hand_urdf_xacro_file:
hand_urdf_xml = parse(hand_urdf_xacro_file)
xacro.process_includes(hand_urdf_xml,
os.path.dirname(sys.argv[0]))
xacro.process_doc(hand_urdf_xml)
hand_urdf = hand_urdf_xml.toprettyxml(indent=' ')
srdfHandGenerator = SRDFHandGenerator(hand_urdf,
load=False,
save=False)
self.hand_srdf_xml = srdfHandGenerator.get_hand_srdf()
comment = ["Manipulator:" + manipulator.name]
self.add_comments(comment)
# Add groups and group states
if manipulator.has_arm:
self.parse_arm_groups(manipulator)
if manipulator.has_hand:
self.parse_hand_groups(manipulator)
# Add end effectors
comment = [
"END EFFECTOR: Purpose: Represent information about an end effector."
]
self.add_comments(comment)
if manipulator.has_hand:
self.parse_hand_end_effectors(manipulator)
if manipulator.has_arm:
self.parse_arm_end_effectors(manipulator)
# Add virtual joints
if manipulator.has_arm:
pass
# self.parse_arm_virtual_joint(manipulator)
else:
self.parse_hand_virtual_joint(manipulator)
# Add disable collisions
comment = [
"DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially "
+
"come into collision with any other link in the robot. This tag disables collision checking "
+ "between a specified pair of links."
]
self.add_comments(comment)
if manipulator.has_arm:
self.parse_arm_collisions(manipulator)
if manipulator.has_hand:
self.parse_hand_collisions(manipulator)
# Finish and close file
self.new_robot_srdf.write('</robot>\n')
self.new_robot_srdf.close()
if load:
with open(self.package_path + "/config/" + new_srdf_file_name,
'r') as stream:
srdf = parse(stream)
rospy.loginfo("Loading SRDF on parameter server")
robot_description_param = rospy.resolve_name(
'robot_description') + "_semantic"
rospy.set_param(robot_description_param,
srdf.toprettyxml(indent=' '))
if self._save_files:
rospy.loginfo("Robot urdf and srdf have been saved to %s" %
self._path_to_save_files)
# srdf: File is already generated so just need to be copied to specified location
copy2(self.package_path + "/config/" + new_srdf_file_name,
self._path_to_save_files + "/" + self._file_name + ".srdf")
# urdf: File can be copied from rosparam
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_file = open(
self._path_to_save_files + "/" + self._file_name + ".urdf",
"wb")
urdf_file.write(urdf_str)
urdf_file.close()
rospy.loginfo("generated_robot.srdf has been generated and saved.")
def __init__(self, urdf_str=None, load=True, save=True):
if urdf_str is None:
while not rospy.has_param('robot_description'):
rospy.sleep(0.5)
rospy.loginfo("waiting for robot_description")
# load the urdf from the parameter server
urdf_str = rospy.get_param('robot_description')
robot = URDF.from_xml_string(urdf_str)
extracted_prefix = False
prefix = ""
ff = mf = rf = lf = th = False
is_lite = True
is_biotac = False
hand_name = "right_hand"
# Check if hand has the old biotac sensors
for key in robot.link_map:
link = robot.link_map[key]
if link.visual:
if hasattr(link.visual.geometry, 'filename'):
filename = os.path.basename(link.visual.geometry.filename)
if filename == "biotac_decimated.dae":
is_biotac = True
break
for key in robot.joint_map:
# any joint is supposed to have the same prefix and a joint name with 4 chars
if not extracted_prefix:
prefix = key.split("_")[0] + "_"
rospy.loginfo("Found prefix:" + prefix)
extracted_prefix = True
if prefix == "lh_":
hand_name = "left_hand"
if not ff and key.endswith("FFJ4"):
ff = True
if not mf and key.endswith("MFJ4"):
mf = True
if not rf and key.endswith("RFJ4"):
rf = True
if not lf and key.endswith("LFJ4"):
lf = True
if not th and key.endswith("THJ4"):
th = True
if is_lite and key.endswith("WRJ2"):
is_lite = False
rospy.logdebug("Found fingers (ff mf rf lf th)" + str(ff) + str(mf) +
str(rf) + str(lf) + str(th))
rospy.logdebug("is_lite: " + str(is_lite))
rospy.logdebug("is_biotac: " + str(is_biotac))
rospy.logdebug("Hand name: " + str(hand_name))
mappings = load_mappings([
'prefix:=' + str(prefix), 'robot_name:=' + robot.name,
'ff:=' + str(int(ff)), 'mf:=' + str(int(mf)),
'rf:=' + str(int(rf)), 'lf:=' + str(int(lf)),
'th:=' + str(int(th)), 'is_lite:=' + str(int(is_lite)),
'is_biotac:=' + str(int(is_biotac)), 'hand_name:=' + str(hand_name)
])
# the prefix version of the srdf_xacro must be loaded
rospack = rospkg.RosPack()
package_path = rospack.get_path('sr_moveit_hand_config')
srdf_xacro_filename = package_path + "/config/shadowhands_prefix.srdf.xacro"
rospy.loginfo("File loaded " + srdf_xacro_filename)
# open and parse the xacro.srdf file
srdf_xacro_file = open(srdf_xacro_filename, 'r')
self.srdf_xacro_xml = parse(srdf_xacro_file)
# expand the xacro
xacro.process_includes(self.srdf_xacro_xml,
os.path.dirname(sys.argv[0]))
xacro.process_doc(self.srdf_xacro_xml, mappings=mappings)
if len(sys.argv) > 1:
OUTPUT_PATH = sys.argv[1]
# reject ROS internal parameters and detect termination
if (OUTPUT_PATH.startswith("_") or OUTPUT_PATH.startswith("--")):
OUTPUT_PATH = None
else:
OUTPUT_PATH = None
if load:
rospy.loginfo("Loading SRDF on parameter server")
robot_description_param = rospy.resolve_name(
'robot_description') + "_semantic"
rospy.set_param(robot_description_param,
self.srdf_xacro_xml.toprettyxml(indent=' '))
if save:
OUTPUT_PATH = package_path + "/config/generated_shadowhand.srdf"
FW = open(OUTPUT_PATH, "wb")
FW.write(self.srdf_xacro_xml.toprettyxml(indent=' '))
FW.close()
OUTPUT_PATH = package_path + "/config/generated_shadowhand.urdf"
FW = open(OUTPUT_PATH, "wb")
FW.write(urdf_str)
FW.close()
srdf_xacro_file.close()