def export_robot_to_xacro_files():
"""
Exports the entire 'robot' in several xacro files.
One xacro file per kinematic chain (<ROBOT>_legs.xacro, <ROBOT>_arms.xacro, <ROBOT>_torso.xacro...)
Xacro file for specific parts of the robot (<ROBOT>_fingers.xacro, <ROBOT>_sensors.xacro)
One xacro file for visual elements (<ROBOT>_visual_collision.xacro, <ROBOT>_material.xacro)
One xacro file per type of element needed for gazebo simulation (<ROBOT>_Gazebo.xacro, <ROBOT>_Transmission.xacro)
One generic robot file which includes all the other ones (<ROBOT>_robot.xacro)
"""
global robot, OUTPUT, NAME
doc = Document()
root = doc.createElement("robot")
doc.appendChild(root)
root.setAttribute("xmlns:xacro","http://www.ros.org/wiki/xacro")
root.setAttribute("name",robot.name)
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_visual_collisions.xacro'))
create_visual_xacro()
for i in XACRO_DICO.keys():
print("exporting " + NAME + '_' + i + '.xacro')
if i.find("eye") != -1:
export_kinematic_chain_to_xacro(i,'HeadRoll_link','HeadRoll_link')
else:
export_kinematic_chain_to_xacro(i)
filenamerobot = NAME + '_' + i + '.xacro'
root.appendChild(ur.short(doc,'xacro:include','filename',filenamerobot))
## Transmission elements not available from Aldebaran libraries yet
# export_robot_element('Transmission')
# root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Transmission.xacro'))
## Gazebo Plugin not available from Aldebaran libraries yet
# export_robot_element('Gazebo')
# root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Gazebo.xacro'))
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_sensors.xacro'))
export_list_to_xacro(['_frame'],OUTPUT[0:OUTPUT.find('.')] + '_sensors.xacro')
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_fingers.xacro'))
export_list_to_xacro(['Finger','Thumb'],OUTPUT[0:OUTPUT.find('.')] + '_fingers.xacro')
if NAME == 'pepper':
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_wheels.xacro'))
export_list_to_xacro(['Wheel'],OUTPUT[0:OUTPUT.find('.')] + '_wheels.xacro')
filename = OUTPUT[0:OUTPUT.find('.')]+ '_robot.xacro'
write_comments_in_xacro(doc,filename)
print('output directory : ' + OUTPUT[0:OUTPUT.rfind('/')+1])
def export_robot_to_xacro_files():
"""
Exports the entire 'robot' in several xacro files.
One xacro file per kinematic chain (<ROBOT>_legs.xacro, <ROBOT>_arms.xacro, <ROBOT>_torso.xacro...)
Xacro file for specific parts of the robot (<ROBOT>_fingers.xacro, <ROBOT>_sensors.xacro)
One xacro file for visual elements (<ROBOT>_visual_collision.xacro, <ROBOT>_material.xacro)
One xacro file per type of element needed for gazebo simulation (<ROBOT>_Gazebo.xacro, <ROBOT>_Transmission.xacro)
One generic robot file which includes all the other ones (<ROBOT>_robot.xacro)
"""
global robot, OUTPUT, NAME
doc = Document()
root = doc.createElement("robot")
doc.appendChild(root)
root.setAttribute("xmlns:xacro","http://www.ros.org/wiki/xacro")
root.setAttribute("name",robot.name)
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_visual_collisions.xacro'))
create_visual_xacro()
for i in XACRO_DICO.keys():
print("exporting " + NAME + '_' + i + '.xacro')
if i.find("eye") != -1:
export_kinematic_chain_to_xacro(i,'HeadRoll_link','HeadRoll_link')
else:
export_kinematic_chain_to_xacro(i)
filenamerobot = NAME + '_' + i + '.xacro'
root.appendChild(ur.short(doc,'xacro:include','filename',filenamerobot))
## Transmission elements not available from Aldebaran libraries yet
# export_robot_element('Transmission')
# root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Transmission.xacro'))
## Gazebo Plugin not available from Aldebaran libraries yet
# export_robot_element('Gazebo')
# root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Gazebo.xacro'))
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_sensors.xacro'))
export_list_to_xacro(['_frame'],OUTPUT[0:OUTPUT.find('.')] + '_sensors.xacro')
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_fingers.xacro'))
export_list_to_xacro(['Finger','Thumb'],OUTPUT[0:OUTPUT.find('.')] + '_fingers.xacro')
if NAME == 'pepper':
root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_wheels.xacro'))
export_list_to_xacro(['Wheel'],OUTPUT[0:OUTPUT.find('.')] + '_wheels.xacro')
filename = OUTPUT[0:OUTPUT.find('.')]+ '_robot.xacro'
write_comments_in_xacro(doc,filename)
print('output directory : ' + OUTPUT[0:OUTPUT.rfind('/')+1])
def create_visual_xacro():
global robot
"""
Creates a <ROBOT>_visual_collision.xacro file with xacro macros for visual and collision tags of the urdf.
This function creates xacro macros for visualisation and collisions. It checks if the meshes are on the computer(nao only) and set the 'meshes_installed' property accordingly
"""
global OUTPUT
global MESH_VERSION
global NAME
global LINKS_DICO
global VISU_DICO
global OFFSETS_DICO
global MESHPKG
prefix = "insert_visu_"
doc = Document()
root = doc.createElement("robot")
doc.appendChild(root)
root.setAttribute("xmlns:xacro","http://www.ros.org/wiki/xacro")
cmd = "rospack find "+ NAME + MESHPKG
try:
path_mesh_pkg = subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True)[:-1]
except:
print("unable to find "+ NAME + MESHPKG + " package")
sys.exit(0)
# Set Mesh path
if NAME =='nao':
node = ur.short(doc,'xacro:property','name','PI_2')
node.setAttribute('value',str(math.pi/2.0))
root.appendChild(node)
node = ur.short(doc,'xacro:property','name','meshes_installed')
if os.path.isdir(os.path.join(path_mesh_pkg,'meshes',MESH_VERSION)):
node.setAttribute('value',"true")
else:
node.setAttribute('value',"false")
root.appendChild(node)
#Insert xacro macro
for link in robot.links:
(tempVisu,tempCol) = adjustMeshPath(path_mesh_pkg,link)
if robot.links[link].visual != None:
robot.links[link].xacro = 'xacro:' + prefix + robot.links[link].name
node = ur.short(doc,'xacro:macro','name', prefix + robot.links[link].name)
if NAME=='nao':
# add xacro condition macro to handle the absence of meshes on the system
node2 = ur.short(doc,'xacro:unless','value',"${meshes_installed}")
if tempVisu != None:
node2.appendChild(tempVisu.to_xml(doc))
if tempCol != None:
node2.appendChild(tempCol.to_xml(doc))
node.appendChild(node2)
node3 = ur.short(doc,'xacro:if','value',"${meshes_installed}")
node3.appendChild(robot.links[link].visual.to_xml(doc))
node3.appendChild(robot.links[link].collision.to_xml(doc))
node.appendChild(node3)
else:
node.appendChild(robot.links[link].visual.to_xml(doc))
node.appendChild(robot.links[link].collision.to_xml(doc))
root.appendChild(node)
robot.links[link].visual = None
robot.links[link].collision = None
filename = OUTPUT[0:OUTPUT.rfind('.')] + '_visual_collisions.xacro'
write_comments_in_xacro(doc, filename)
def create_visual_xacro():
"""Create a <ROBOT>_visual_collision.xacro file.
with xacro macros for visual and collision tags of the urdf.
This function creates xacro macros for visualisation and collisions.
It checks if the meshes are on the computer and set the 'meshes_installed'
property accordingly
"""
global robot
global OUTPUT
global MESH_VERSION
global NAME
global LINKS_DICO
global VISU_DICO
global OFFSETS_DICO
global MESHPKG
prefix = 'insert_visu_'
doc = Document()
root = doc.createElement('robot')
doc.appendChild(root)
root.setAttribute("xmlns:xacro", "http://www.ros.org/wiki/xacro")
cmd = 'rospack find ' + NAME + MESHPKG
try:
path_mesh_pkg = subprocess.check_output(cmd, stderr=subprocess.STDOUT,
shell=True)[:-1]
except:
print('unable to find ' + NAME + MESHPKG + ' package')
sys.exit(0)
# Set Mesh path
if NAME == 'nao':
node = ur.short(doc, 'xacro:property', 'name', 'PI_2')
node.setAttribute('value', str(math.pi/2.0))
root.appendChild(node)
node = ur.short(doc, 'xacro:property', 'name', 'meshes_installed')
if os.path.isdir(os.path.join(path_mesh_pkg, 'meshes', MESH_VERSION)):
node.setAttribute('value', 'true')
else:
node.setAttribute('value', 'false')
root.appendChild(node)
# Insert xacro macro
for link in robot.links:
(tempVisu, tempCol) = adjustMeshPath(path_mesh_pkg, link)
if robot.links[link].visual is not None:
robot.links[link].xacro = 'xacro:' + prefix + \
robot.links[link].name
node = ur.short(doc, 'xacro:macro', 'name', prefix +
robot.links[link].name)
if NAME == 'nao':
# add xacro condition macro to handle the absence of meshes
node2 = ur.short(doc, 'xacro:unless', 'value',
'${meshes_installed}')
if tempVisu is not None:
node2.appendChild(tempVisu.to_xml(doc))
if tempCol is not None:
node2.appendChild(tempCol.to_xml(doc))
node.appendChild(node2)
node3 = ur.short(doc, 'xacro:if', 'value', '${meshes_installed}')
node3.appendChild(robot.links[link].visual.to_xml(doc))
node3.appendChild(robot.links[link].collision.to_xml(doc))
node.appendChild(node3)
else:
node.appendChild(robot.links[link].visual.to_xml(doc))
node.appendChild(robot.links[link].collision.to_xml(doc))
root.appendChild(node)
robot.links[link].visual = None
robot.links[link].collision = None
filename = OUTPUT[0:OUTPUT.rfind('.')] + '_visual_collisions.xacro'
write_comments_in_xacro(doc, filename)
