def setUp(self):
self.recorder_launch_file_path = resolve_args(
"$(find mode_manager)/examples/launch/recorder.launch")
self.assertTrue(os.path.exists(self.recorder_launch_file_path))
self.param_launch_file_path = resolve_args(
"$(find mode_manager)/examples/launch/set_param_to_arg.launch")
self.assertTrue(os.path.exists(self.param_launch_file_path))
def _get_arg_value(tag, context):
name = tag.attributes['name'].value
if tag.attributes.has_key('value'):
return resolve_args(tag.attributes['value'].value, context)
elif name in context['arg']:
return context['arg'][name]
elif tag.attributes.has_key('default'):
return resolve_args(tag.attributes['default'].value, context)
else:
raise RoslaunchDepsException("No value for arg [%s]"%(name))
def _check_ifunless(tag, context):
if tag.attributes.has_key('if'):
val = resolve_args(tag.attributes['if'].value, context)
if not convert_value(val, 'bool'):
return False
elif tag.attributes.has_key('unless'):
val = resolve_args(tag.attributes['unless'].value, context)
if convert_value(val, 'bool'):
return False
return True
def _get_arg_value(tag, context):
name = tag.attributes['name'].value
if 'value' in tag.attributes.keys():
return resolve_args(tag.attributes['value'].value, context)
elif name in context['arg']:
return context['arg'][name]
elif 'default' in tag.attributes.keys():
return resolve_args(tag.attributes['default'].value, context)
else:
raise RoslaunchDepsException("No value for arg [%s]"%(name))
def _check_ifunless(tag, context):
if 'if' in tag.attributes.keys():
val = resolve_args(tag.attributes['if'].value, context)
if not convert_value(val, 'bool'):
return False
elif 'unless' in tag.attributes.keys():
val = resolve_args(tag.attributes['unless'].value, context)
if convert_value(val, 'bool'):
return False
return True
def eval_extension(s): # pragma: no cover
if s == '$(cwd)':
return os.getcwd()
if s.startswith('$(find'):
return tld
# return '..'
if s.startswith('$(arg'):
s = s.replace('$(arg ', '')
s = s[:-1]
return substitution_args_context['arg'][s]
try:
from roslaunch.substitution_args import resolve_args, ArgException
from rospkg.common import ResourceNotFound
return resolve_args(s,
context=substitution_args_context,
resolve_anon=False)
except ImportError:
return ''
# raise XacroException("substitution args not supported: ", exc=e)
except ArgException as e:
raise XacroException("Undefined substitution argument", exc=e)
except ResourceNotFound as e:
raise XacroException("resource not found:", exc=e)
def eval_extension(s):
if s == '$(cwd)':
return os.getcwd()
try:
return substitution_args.resolve_args(s, context=substitution_args_context, resolve_anon=False)
except substitution_args.ArgException as e:
raise XacroException("Undefined substitution argument", exc=e)
except ResourceNotFound as e:
raise XacroException("resource not found:", exc=e)
def eval_extension(s):
if s == '$(cwd)':
return os.getcwd()
try:
return substitution_args.resolve_args(s, context=substitution_args_context, resolve_anon=False)
except substitution_args.ArgException as e:
raise XacroException("Undefined substitution argument", exc=e)
except ResourceNotFound as e:
raise XacroException("resource not found:", exc=e)
def parse_launch_file(launch_file):
'''
args: launchfile
output: uwsim dataPath and scene file
'''
launch_xml = etree.parse(launch_file)
uwsim_node = [
node for sub in ['', 'group/']
for node in launch_xml.findall(sub + 'node')
if node.get('type') == 'uwsim'
]
# check node existence
if len(uwsim_node) == 0:
rospy.loginfo('No uwsim node found')
sys.exit(0)
elif len(uwsim_node) > 2:
rospy.loginfo('Found several uwsim node, check launchfile')
sys.exit(1)
# create mapping between uwsim dataPath and absolute/ros paths
datapath = [roslib.packages.get_pkg_dir('uwsim') + '/data']
local_datapath = os.path.expanduser('~/.uwsim/data')
if os.path.lexists(local_datapath):
datapath.append(local_datapath)
# now we have the uwsim node, let's substitute and parse its args
# we cannot substitute $(arg XXX) outside the launchfile, so we just remove it
uwsim_args = substitution_args.resolve_args(
uwsim_node[0].get('args').replace('$(arg', '')).split(' ')
print "uwsim_args: ", uwsim_args
# look for configfile and additional datapath in uwsim args
configfile = ''
for i in xrange(len(uwsim_args)):
if uwsim_args[i] == '--configfile':
i += 1
configfile = uwsim_args[i]
elif uwsim_args[i] == '--dataPath':
i += 1
datapath.append(uwsim_args[i])
# check scene file
if configfile == '':
rospy.loginfo('No scene file found in launchfile')
sys.exit(0)
configfile, datadir = uwsim_to_abspath(configfile, datapath)
if datadir == '':
rospy.loginfo('Scene file ' + configfile +
' not found in uwsim dataPath')
return datapath, configfile
def eval_extension(s):
if s == '$(cwd)':
return os.getcwd()
try:
from roslaunch import substitution_args
from rospkg.common import ResourceNotFound
return substitution_args.resolve_args(s, context=substitution_args_context, resolve_anon=False)
except ImportError as e:
raise XacroException("substitution args not supported: ", exc=e)
except substitution_args.ArgException as e:
raise XacroException("Undefined substitution argument", exc=e)
except ResourceNotFound as e:
raise XacroException("resource not found:", exc=e)
def _load_roslaunch(self, config):
args = config.get(self.ARG_ARGS, None)
launch_file = resolve_args(config[self.ARG_LAUNCH_FILE])
if not os.path.exists(launch_file):
raise ControllerException("Lanch file %s (resolved from %s) for roslaunch config action does not exist" %
(launch_file, config[self.ARG_LAUNCH_FILE]))
command = [self.COMMAND_ROSLAUNCH, launch_file]
if args is not None:
if isinstance(args, list):
command.extend(args)
else:
command.append(args)
return command
def test_roslaunch_controller(self):
pc = EntryExitProcessConfigAction([
"roslaunch",
resolve_args(
"$(find mode_manager)/examples/launch/set_param_to_arg.launch"
), "value:=foo"
])
try:
rospy.get_param("/test/param")
self.fail("should have thrown key error")
except KeyError as e:
pass
pc.activate()
value = rospy.get_param("/test/param")
self.assertEquals("foo", value)
def eval_extension(s): # pragma: no cover
if s == '$(cwd)':
return os.getcwd()
try:
from roslaunch.substitution_args import resolve_args, ArgException
from rospkg.common import ResourceNotFound
return resolve_args(s,
context=substitution_args_context,
resolve_anon=False)
except ImportError:
pass
# raise XacroException("substitution args not supported: ", exc=e)
except ArgException as e:
raise XacroException("Undefined substitution argument", exc=e)
except ResourceNotFound as e:
raise XacroException("resource not found:", exc=e)
def parse_launch_file(launch_file):
'''
args: launchfile
output: uwsim dataPath and scene file
'''
launch_xml = etree.parse(launch_file)
uwsim_node = [node for sub in ['', 'group/'] for node in launch_xml.findall(sub+'node') if node.get('type') == 'uwsim']
# check node existence
if len(uwsim_node) == 0:
rospy.loginfo('No uwsim node found')
sys.exit(0)
elif len(uwsim_node) > 1:
rospy.loginfo('Found several uwsim node, check launchfile')
sys.exit(1)
# create mapping between uwsim dataPath and absolute/ros paths
datapath = [roslib.packages.get_pkg_dir('uwsim')+'/data']
local_datapath = os.path.expanduser('~/.uwsim/data')
if os.path.lexists(local_datapath):
datapath.append(local_datapath)
# now we have the uwsim node, let's substitute and parse its args
# we cannot substitute $(arg XXX) outside the launchfile, so we just remove it
uwsim_args = substitution_args.resolve_args(uwsim_node[0].get('args').replace('$(arg', '')).split(' ')
# look for configfile and additional datapath in uwsim args
configfile = ''
for i in xrange(len(uwsim_args)):
if uwsim_args[i] == '--configfile':
i+= 1
configfile = uwsim_args[i]
elif uwsim_args[i] == '--dataPath':
i+= 1
datapath.append(uwsim_args[i])
# check scene file
if configfile == '':
rospy.loginfo('No scene file found in launchfile')
sys.exit(0)
configfile, datadir = uwsim_to_abspath(configfile, datapath)
if datadir == '':
rospy.loginfo('Scene file ' + configfile + ' not found in uwsim dataPath')
return datapath, configfile
def abspath_to_roslaunch(filename):
'''
Tells whether the filename lies in rospath, $home, or elsewhere
'''
filetree = filename.split('/')
# test for ros paths
for d in range(len(filetree)-1,-1,-1):
package_path = ''
try:
package_path = substitution_args.resolve_args('$(find %s)' % filetree[d])
except:
pass
if package_path != '':
if package_path == '/'.join(filetree[:d+1]):
return '$(find %s)/%s' % (filetree[d], '/'.join(filetree[d+1:]))
# test for home dir
userpath = os.path.expanduser('~')
if userpath in filename:
return '$(env HOME)/%s' % filename[len(userpath)+1:]
# other
return filename
def abspath_to_roslaunch(filename):
'''
Tells whether the filename lies in rospath, $home, or elsewhere
'''
filetree = filename.split('/')
# test for ros paths
for d in range(len(filetree)-1,-1,-1):
package_path = ''
try:
package_path = substitution_args.resolve_args('$(find %s)' % filetree[d])
except:
pass
if package_path != '':
if package_path == '/'.join(filetree[:d+1]):
return '$(find %s)/%s' % (filetree[d], '/'.join(filetree[d+1:]))
# test for home dir
userpath = os.path.expanduser('~')
if userpath in filename:
return '$(env HOME)/%s' % filename[len(userpath)+1:]
# other
return filename
def process_vehicles(vehicles, datapath):
'''
Looks for the vehicle file in datapath
Resolves corresponding urdf or xacro
- convention: uwsim urdf files come from Gazebo urdf files
- XXX.urdf or XXX.xacro -> XXX_uwsim.urdf
Creates Gazebo spawner with namespace
'''
vehicle_nodes = []
for vehicle in vehicles:
name = vehicle.findtext('name')
uwsim_urdf_file = vehicle.findtext('file')
# get to Gazebo xacro/urdf file
gazebo_model_file, datadir = uwsim_to_abspath(uwsim_urdf_file.replace('_uwsim.urdf','.xacro'), datapath)
if datadir == '':
gazebo_model_file, datadir = uwsim_to_abspath(uwsim_urdf_file.replace('_uwsim',''), datapath)
if datadir == '':
rospy.loginfo('Could not find original file for ' + uwsim_urdf_file + ' in ' + scene_file)
sys.exit(1)
uwsim_urdf_file = gazebo_model_file.replace('.urdf', '_uwsim.urdf').replace('.xacro', '_uwsim.urdf')
# parse Gazebo file (urdf or xacro)
if 'xacro' in gazebo_model_file:
uwsim_urdf_xml = subprocess.Popen(['rosrun', 'xacro', 'xacro', gazebo_model_file], stdout=subprocess.PIPE)
uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.stdout.read())
# uwsim_urdf_xml = xacro.parse(gazebo_model_file)
# xacro.eval_self_contained(uwsim_urdf_xml)
# uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.toxml())
else:
uwsim_urdf_xml = etree.parse(gazebo_model_file).getroot()
# clean up URDF: keep only joints and links
for child in uwsim_urdf_xml.getchildren():
if child.tag not in ('joint', 'link'):
uwsim_urdf_xml.remove(child)
# and remove collision / inertial / buoyancy from links
to_be_removed = ('collision', 'inertial', 'buoyancy')
for link in uwsim_urdf_xml.findall('link'):
for tag in to_be_removed:
for node in link.findall(tag):
link.remove(node)
# change mesh url-filenames to uwsim relative filenames
meshes = uwsim_urdf_xml.findall('link/visual/geometry/mesh')
if len(meshes) != 0:
# create uwsim urdf only if mesh in gazebo urdf, otherwise trust the user
for mesh in meshes:
mesh_file = resource_retriever.get(substitution_args.resolve_args(mesh.get('filename'))).url[7:]
# get uwsim relative path for mesh
mesh_file, datadir = abspath_to_uwsim(mesh_file, datapath)
# if mesh in dae, try to find corresponding osg
if '.dae' in mesh_file:
mesh_osg, datadir_osg = uwsim_to_abspath(mesh_file.replace('.dae','.osg'), datapath)
if datadir_osg != '':
mesh_file, datadir = abspath_to_uwsim(mesh_osg, datapath)
if datadir == '':
rospy.loginfo('Could not find relative path for ' + mesh_file + ' in ' + gazebo_model_file)
sys.exit(1)
else:
if os.path.lexists(datadir + '/' + os.path.splitext(mesh_file)[0] + '.osg'):
mesh_file = mesh_file.split('.')[0] + '.osg'
mesh.set('filename', mesh_file)
# write uwsim urdf
insert_header_and_write(uwsim_urdf_xml, gazebo_model_file, uwsim_urdf_file)
# get nodes to write in gazebo spawner
vehicle_nodes.append(create_spawner(vehicle, gazebo_model_file))
return vehicle_nodes
def eval_extension(str):
return substitution_args.resolve_args(str,
context=substitution_args_context,
resolve_anon=False)
from lxml import etree
import yaml
from numpy import array
if __name__ == '__main__':
p_default = 2
i_default = 0
d_default = 0
if len(sys.argv) < 2:
print 'Input a .xacro or .urdf file'
print 'syntax : gen_pid.py <package> <urdf file>'
sys.exit(0)
robot_package = substitution_args.resolve_args('$(find %s)' % sys.argv[1])
robot_file = sys.argv[2]
# find file
robot_abs_file = ''
for urdf_dir in ['urdf', 'sdf']:
if os.path.lexists('%s/%s/%s' % (robot_package, urdf_dir, robot_file)):
robot_abs_file = '%s/%s/%s' % (robot_package, urdf_dir, robot_file)
# create config directory
if not os.path.lexists('%s/config' % robot_package):
os.mkdir('%s/config' % robot_package)
config_file = '%s/config/%s_pid.yaml' % (robot_package, robot_file.split('.')[0])
# load description
robot_description = etree.fromstring(check_output(['rosrun', 'xacro', 'xacro', robot_abs_file]))
def test_resolve_args():
from roslaunch.substitution_args import resolve_args, SubstitutionException
r = rospkg.RosPack()
roslaunch_dir = r.get_path('roslaunch')
assert roslaunch_dir
anon_context = {'foo': 'bar'}
arg_context = {'fuga': 'hoge', 'car': 'cdr', 'arg': 'foo', 'True': 'False'}
context = {'anon': anon_context, 'arg': arg_context }
tests = [
('$(find roslaunch)', roslaunch_dir),
('hello$(find roslaunch)', 'hello'+roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$$(find roslaunch )', '$'+roslaunch_dir),
('$( find roslaunch )', roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$(find roslaunch)$(find roslaunch)', roslaunch_dir+os.sep+roslaunch_dir),
('$(find roslaunch)/foo/bar.xml', roslaunch_dir+os.sep+'foo'+os.sep+'bar.xml'),
(r'$(find roslaunch)\foo\bar.xml $(find roslaunch)\bar.xml', roslaunch_dir+os.sep+'foo'+os.sep+'bar.xml '+roslaunch_dir+os.sep+'bar.xml'),
('$(find roslaunch)\\foo\\bar.xml more/stuff\\here', roslaunch_dir+os.sep+'foo'+os.sep+'bar.xml more/stuff\\here'),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)$(optenv ROS_ROOT)', os.environ['ROS_ROOT']+os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
('$(optenv NOT_ROS_ROOT)', ''),
('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),
# #1776
('$(anon foo)', 'bar'),
('$(anon foo)/baz', 'bar/baz'),
('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),
# arg
('$(arg fuga)', 'hoge'),
('$(arg fuga)$(arg fuga)', 'hogehoge'),
('$(arg car)$(arg fuga)', 'cdrhoge'),
('$(arg fuga)hoge', 'hogehoge'),
# $(eval ...) versions of those tests
("$(eval find('roslaunch'))", roslaunch_dir),
("$(eval env('ROS_ROOT'))", os.environ['ROS_ROOT']),
("$(eval optenv('ROS_ROOT', 'alternate text'))", os.environ['ROS_ROOT']),
("$(eval optenv('NOT_ROS_ROOT', 'alternate text'))", "alternate text"),
("$(eval optenv('NOT_ROS_ROOT'))", ""),
("$(eval anon('foo'))", 'bar'),
("$(eval arg('fuga'))", 'hoge'),
('$(eval arg("fuga"))', 'hoge'),
('$(eval arg("arg"))', 'foo'),
('$(eval arg("True"))', 'False'),
('$(eval 1==1)', 'True'),
('$(eval [0,1,2][1])', '1'),
# test implicit arg access
('$(eval fuga)', 'hoge'),
('$(eval True)', 'True'),
# math expressions
('$(eval round(sin(pi),1))', '0.0'),
('$(eval cos(0))', '1.0'),
# str, map
("$(eval ''.join(map(str, [4,2])))", '42'),
]
for arg, val in tests:
assert val == resolve_args(arg, context=context), arg
# more #1776
r = resolve_args('$(anon foo)/bar')
assert '/bar' in r
assert not '$(anon foo)' in r
# test against strings that should not match
noop_tests = [
'$(find roslaunch', '$find roslaunch', '', ' ', 'noop', 'find roslaunch', 'env ROS_ROOT', '$$', ')', '(', '()',
None,
]
for t in noop_tests:
assert t == resolve_args(t)
failures = [
'$((find roslaunch))', '$(find $roslaunch)',
'$(find)', '$(find roslaunch roslaunch)', '$(export roslaunch)',
'$(env)', '$(env ROS_ROOT alternate)',
'$(env NOT_SET)',
'$(optenv)',
'$(anon)',
'$(anon foo bar)',
]
for f in failures:
try:
resolve_args(f)
assert False, "resolve_args(%s) should have failed"%f
except SubstitutionException: pass
def process_vehicles(vehicles, datapath):
'''
Looks for the vehicle file in datapath
Resolves corresponding urdf or xacro
- convention: uwsim urdf files come from Gazebo urdf files
- XXX.urdf or XXX.xacro -> XXX_uwsim.urdf
Creates Gazebo spawner with namespace
'''
vehicle_nodes = []
for vehicle in vehicles:
name = vehicle.findtext('name')
uwsim_urdf_file = vehicle.findtext('file')
print "---> vehicle: " + name + " file: " + uwsim_urdf_file
# get to Gazebo xacro/urdf file
gazebo_model_file, datadir = uwsim_to_abspath(uwsim_urdf_file.replace('_uwsim.urdf','.xacro').replace("urdf/","xacro/"), datapath)
if datadir == '':
gazebo_model_file, datadir = uwsim_to_abspath(uwsim_urdf_file.replace('_uwsim',''), datapath)
if datadir == '':
rospy.loginfo('Could not find original file for ' + uwsim_urdf_file + ' in ' + scene_file)
sys.exit(1)
uwsim_urdf_file = gazebo_model_file.replace('.urdf', '_uwsim.urdf').replace('.xacro', '_uwsim.urdf').replace("xacro/", "urdf/")
print "gazebo_model_file: "+gazebo_model_file
# parse Gazebo file (urdf or xacro)
if 'xacro' in gazebo_model_file:
print "running xacro"
uwsim_urdf_xml = subprocess.Popen(['rosrun', 'xacro', 'xacro', '--inorder', gazebo_model_file], stdout=subprocess.PIPE)
uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.stdout.read())
# uwsim_urdf_xml = xacro.parse(gazebo_model_file)
# xacro.eval_self_contained(uwsim_urdf_xml)
# uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.toxml())
else:
uwsim_urdf_xml = etree.parse(gazebo_model_file).getroot()
print "parsing model file"
# clean up URDF: keep only joints and links
for child in uwsim_urdf_xml.getchildren():
if child.tag not in ('joint', 'link'):
uwsim_urdf_xml.remove(child)
# and remove collision / inertial / buoyancy from links
to_be_removed = ('collision', 'inertial', 'buoyancy')
for link in uwsim_urdf_xml.findall('link'):
for tag in to_be_removed:
for node in link.findall(tag):
link.remove(node)
# change mesh url-filenames to uwsim relative filenames
meshes = uwsim_urdf_xml.findall('link/visual/geometry/mesh')
if len(meshes) != 0:
# create uwsim urdf only if mesh in gazebo urdf, otherwise trust the user
for mesh in meshes:
print "------ mesh: "+ mesh.get('filename')
print "------ " + substitution_args.resolve_args(mesh.get('filename'))
#mesh_file = resource_retriever.get(substitution_args.resolve_args(mesh.get('filename'))).url[7:]
mesh_file = substitution_args.resolve_args(mesh.get('filename'))
#print mesh_file
print "datapath: ",datapath;
#mesh_file = substitution_args.resolve_args(mesh.get('filename'))
# get uwsim relative path for mesh
mesh_file, datadir = abspath_to_uwsim(mesh_file, datapath)
#mesh_file, datadir = uwsim_to_abspath(mesh_file, datapath)
print "======= novooo: "
print mesh_file
# if mesh in dae, try to find corresponding osg
if '.dae' in mesh_file:
mesh_osg, datadir_osg = uwsim_to_abspath(mesh_file.replace('.dae','.obj'), datapath)
if datadir_osg != '':
mesh_file, datadir = abspath_to_uwsim(mesh_osg, datapath)
if '.stl' in mesh_file:
mesh_osg, datadir_osg = uwsim_to_abspath(mesh_file.replace('.stl','.obj'), datapath)
if datadir_osg != '':
mesh_file, datadir = abspath_to_uwsim(mesh_osg, datapath)
if datadir == '':
rospy.loginfo('Could not find relative path for ' + mesh_file + ' in ' + gazebo_model_file)
sys.exit(1)
else:
if os.path.lexists(datadir + '/' + os.path.splitext(mesh_file)[0] + '.osg'):
mesh_file = mesh_file.split('.')[0] + '.osg'
mesh.set('filename', mesh_file)
# write uwsim urdf
insert_header_and_write(uwsim_urdf_xml, gazebo_model_file, uwsim_urdf_file)
# get nodes to write in gazebo spawner
vehicle_nodes.append(create_spawner(vehicle, gazebo_model_file))
return vehicle_nodes
def eval_extension(str):
if str == '$(cwd)':
return os.getcwd()
return substitution_args.resolve_args(str,
context=substitution_args_context,
resolve_anon=False)
def parse(pkg, model):
gains = {'p': 2, 'i': 0, 'd': 0}
robot_package = substitution_args.resolve_args('$(find %s)' % pkg)
robot_file = os.path.splitext(model)[0]
# find file
robot_abs_file = ''
for urdf_dir in ['urdf', 'sdf']:
for ext in ('urdf','xacro'):
candidate = '{}/{}/{}.{}'.format(robot_package, urdf_dir, robot_file, ext)
if os.path.lexists(candidate):
robot_abs_file = candidate
print('Using model ' + robot_abs_file)
# create config directory
if not os.path.lexists('%s/config' % robot_package):
os.mkdir('%s/config' % robot_package)
# look for existing config file
config_file = ''
for cf in os.listdir('{}/config'.format(robot_package)):
if robot_file in cf:
with open('{}/config/{}'.format(robot_package,cf)) as f:
content = yaml.load(f)
if 'controllers' in content:
config_file = '{}/config/{}'.format(robot_package,cf)
if config_file == '':
config_file = '{}/config/{}_pid.yaml'.format(robot_package, robot_file)
print('Using config ' + config_file)
# load description
robot_description = etree.fromstring(check_output(['rosrun', 'xacro', 'xacro', '--inorder', robot_abs_file]))
# init config dictionary
if os.path.lexists(config_file):
with open(config_file) as f:
pid = yaml.load(f)['controllers']
else:
pid = {'config': {}}
# parse joints
joints = [joint for joint in robot_description.findall('joint') if joint.get('type') != "fixed"]
if len(joints) != 0:
write(pid, 'config/joints/state', 'joint_states')
write(pid, 'config/joints/setpoint', 'joint_setpoint')
write(pid, 'config/joints/command', 'joint_command')
write(pid, 'config/joints/cascaded_position', False)
write(pid, 'config/joints/dynamic_reconfigure', True)
for joint in joints:
for mode in ('position','velocity'):
for g in gains:
write(pid, '{}/{}/{}'.format(joint.get('name'), mode, g), gains[g])
# parse thrusters
thrusters = []
base_link = 'base_link'
damping = [100. for i in range(6)]
m = 5
for gazebo in robot_description.findall('gazebo'):
for plugin in gazebo.findall('plugin'):
if plugin.get('name') == 'freefloating_gazebo_control':
thrusters += plugin.findall('thruster')
if plugin.find('link') != None:
base_link = plugin.find('link').text
if len(thrusters) != 0:
write(pid, 'config/body/state', 'body_state')
write(pid, 'config/body/setpoint', 'body_setpoint')
write(pid, 'config/body/command', 'body_command')
write(pid, 'config/body/cascaded_position', False)
write(pid, 'config/body/dynamic_reconfigure', True)
# get thruster characteristics
T = []
Umax = []
for thruster in thrusters:
thruster_map = [float(v) for v in thruster.find('map').text.split(' ')]
T.append(thruster_map)
Umax.append(float(thruster.find('effort').text))
T = np.matrix(T).transpose()
n = len(Umax)
Umax = np.matrix(Umax).reshape(n,1)
Fmax = np.abs(T)*Umax
Fmax = [Fmax[i,0] for i in range(6)]
body_axis = {0: 'x', 1: 'y', 2: 'z', 3: 'roll', 4: 'pitch', 5: 'yaw'}
for i in range(6):
if Fmax[i]:
for mode in ('position','velocity'):
for g in gains:
write(pid, '{}/{}/{}'.format(body_axis[i], mode, g), gains[g])
# get damping
for link in robot_description.findall('link'):
if link.get('name') == base_link:
damp = link.find('buoyancy').find('damping')
damping = [float(v) for v in damp.get('xyz').split(' ') + damp.get('rpy').split(' ')]
m = float(link.find('inertial').find('mass').get('value'))
mass = [m,m,m]
for key in 'ixx','iyy','izz':
mass.append(float(link.find('inertial').find('inertia').get('ixx')))
with open(config_file, 'w') as f:
yaml.dump({'controllers': pid}, f, default_flow_style=False)
return pid, [Umax[i,0] for i in range(n)], Fmax, mass, damping, config_file
def test_resolve_args():
from roslaunch.substitution_args import resolve_args, SubstitutionException
r = rospkg.RosPack()
roslaunch_dir = r.get_path('roslaunch')
assert roslaunch_dir
anon_context = {'foo': 'bar'}
arg_context = {'fuga': 'hoge', 'car': 'cdr'}
context = {'anon': anon_context, 'arg': arg_context }
tests = [
('$(find roslaunch)', roslaunch_dir),
('hello$(find roslaunch)', 'hello'+roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$$(find roslaunch )', '$'+roslaunch_dir),
('$( find roslaunch )', roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$(find roslaunch)$(find roslaunch)', roslaunch_dir+os.sep+roslaunch_dir),
('$(find roslaunch)/foo/bar.xml', roslaunch_dir+os.sep+'foo'+os.sep+'bar.xml'),
(r'$(find roslaunch)\foo\bar.xml $(find roslaunch)\bar.xml', roslaunch_dir+os.sep+'foo'+os.sep+'bar.xml '+roslaunch_dir+os.sep+'bar.xml'),
('$(find roslaunch)\\foo\\bar.xml more/stuff\\here', roslaunch_dir+os.sep+'foo'+os.sep+'bar.xml more/stuff\\here'),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)$(optenv ROS_ROOT)', os.environ['ROS_ROOT']+os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
('$(optenv NOT_ROS_ROOT)', ''),
('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),
# #1776
('$(anon foo)', 'bar'),
('$(anon foo)/baz', 'bar/baz'),
('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),
# arg
('$(arg fuga)', 'hoge'),
('$(arg fuga)$(arg fuga)', 'hogehoge'),
('$(arg car)$(arg fuga)', 'cdrhoge'),
('$(arg fuga)hoge', 'hogehoge'),
]
for arg, val in tests:
assert val == resolve_args(arg, context=context)
# more #1776
r = resolve_args('$(anon foo)/bar')
assert '/bar' in r
assert not '$(anon foo)' in r
# test against strings that should not match
noop_tests = [
'$(find roslaunch', '$find roslaunch', '', ' ', 'noop', 'find roslaunch', 'env ROS_ROOT', '$$', ')', '(', '()',
None,
]
for t in noop_tests:
assert t == resolve_args(t)
failures = [
'$((find roslaunch))', '$(find $roslaunch)',
'$(find)', '$(find roslaunch roslaunch)', '$(export roslaunch)',
'$(env)', '$(env ROS_ROOT alternate)',
'$(env NOT_SET)',
'$(optenv)',
'$(anon)',
'$(anon foo bar)',
]
for f in failures:
try:
resolve_args(f)
assert False, "resolve_args(%s) should have failed"%f
except SubstitutionException: pass
from lxml import etree
import yaml
from numpy import array
if __name__ == '__main__':
p_default = 2
i_default = 0
d_default = 0
if len(sys.argv) < 2:
print 'Input a .xacro or .urdf file'
print 'syntax : gen_pid.py <package> <urdf file>'
sys.exit(0)
robot_package = substitution_args.resolve_args('$(find %s)' % sys.argv[1])
robot_file = sys.argv[2]
# find file
robot_abs_file = ''
for urdf_dir in ['urdf', 'sdf']:
if os.path.lexists('%s/%s/%s' % (robot_package, urdf_dir, robot_file)):
robot_abs_file = '%s/%s/%s' % (robot_package, urdf_dir, robot_file)
# create config directory
if not os.path.lexists('%s/config' % robot_package):
os.mkdir('%s/config' % robot_package)
config_file = '%s/config/%s_pid.yaml' % (robot_package,
robot_file.split('.')[0])
# load description
def test_resolve_args():
from roslaunch.substitution_args import resolve_args, SubstitutionException
r = rospkg.RosPack()
roslaunch_dir = r.get_path('roslaunch')
assert roslaunch_dir
anon_context = {'foo': 'bar'}
arg_context = {'fuga': 'hoge', 'car': 'cdr', 'arg': 'foo', 'True': 'False'}
context = {
'anon': anon_context,
'arg': arg_context,
'filename': '/path/to/file.launch'
}
tests = [
('$(find roslaunch)', roslaunch_dir),
('hello$(find roslaunch)', 'hello' + roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$$(find roslaunch )', '$' + roslaunch_dir),
('$( find roslaunch )', roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$(find roslaunch)$(find roslaunch)',
roslaunch_dir + os.sep + roslaunch_dir),
('$(find roslaunch)/foo/bar.xml',
roslaunch_dir + os.sep + 'foo' + os.sep + 'bar.xml'),
(r'$(find roslaunch)\foo\bar.xml $(find roslaunch)\bar.xml',
roslaunch_dir + os.sep + 'foo' + os.sep + 'bar.xml ' + roslaunch_dir +
os.sep + 'bar.xml'),
('$(find roslaunch)\\foo\\bar.xml more/stuff\\here',
roslaunch_dir + os.sep + 'foo' + os.sep + 'bar.xml more/stuff\\here'),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)$(optenv ROS_ROOT)',
os.environ['ROS_ROOT'] + os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
('$(optenv NOT_ROS_ROOT)', ''),
('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),
('$(dirname)/foo', '/path/to/foo'),
# #1776
('$(anon foo)', 'bar'),
('$(anon foo)/baz', 'bar/baz'),
('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),
# arg
('$(arg fuga)', 'hoge'),
('$(arg fuga)$(arg fuga)', 'hogehoge'),
('$(arg car)$(arg fuga)', 'cdrhoge'),
('$(arg fuga)hoge', 'hogehoge'),
# $(eval ...) versions of those tests
("$(eval find('roslaunch'))", roslaunch_dir),
("$(eval env('ROS_ROOT'))", os.environ['ROS_ROOT']),
("$(eval optenv('ROS_ROOT', 'alternate text'))",
os.environ['ROS_ROOT']),
("$(eval optenv('NOT_ROS_ROOT', 'alternate text'))", "alternate text"),
("$(eval optenv('NOT_ROS_ROOT'))", ""),
("$(eval anon('foo'))", 'bar'),
("$(eval arg('fuga'))", 'hoge'),
('$(eval arg("fuga"))', 'hoge'),
('$(eval arg("arg"))', 'foo'),
('$(eval arg("True"))', 'False'),
('$(eval 1==1)', 'True'),
('$(eval [0,1,2][1])', '1'),
# test implicit arg access
('$(eval fuga)', 'hoge'),
('$(eval True)', 'True'),
# math expressions
('$(eval round(sin(pi),1))', '0.0'),
('$(eval cos(0))', '1.0'),
# str, map
("$(eval ''.join(map(str, [4,2])))", '42'),
]
for arg, val in tests:
assert val == resolve_args(arg, context=context), arg
# more #1776
r = resolve_args('$(anon foo)/bar')
assert '/bar' in r
assert not '$(anon foo)' in r
# test against strings that should not match
noop_tests = [
'$(find roslaunch',
'$find roslaunch',
'',
' ',
'noop',
'find roslaunch',
'env ROS_ROOT',
'$$',
')',
'(',
'()',
None,
]
for t in noop_tests:
assert t == resolve_args(t)
failures = [
'$((find roslaunch))',
'$(find $roslaunch)',
'$(find)',
'$(find roslaunch roslaunch)',
'$(export roslaunch)',
'$(env)',
'$(env ROS_ROOT alternate)',
'$(env NOT_SET)',
'$(optenv)',
'$(anon)',
'$(anon foo bar)',
# Should fail without the supplied context.
'$(dirname)'
]
for f in failures:
try:
resolve_args(f)
assert False, "resolve_args(%s) should have failed" % f
except SubstitutionException:
pass
def eval_extension(str):
return substitution_args.resolve_args(str, context=substitution_args_context, resolve_anon=False)
def _parse_launch(tags, launch_file, file_deps, verbose, context):
dir_path = os.path.dirname(os.path.abspath(launch_file))
launch_file_pkg = rospkg.get_package_name(dir_path)
# process group, include, node, and test tags from launch file
for tag in [t for t in tags if t.nodeType == DomNode.ELEMENT_NODE]:
if not _check_ifunless(tag, context):
continue
if tag.tagName == 'group':
#descend group tags as they can contain node tags
_parse_launch(tag.childNodes, launch_file, file_deps, verbose, context)
elif tag.tagName == 'arg':
context['arg'][tag.attributes['name'].value] = _get_arg_value(tag, context)
elif tag.tagName == 'include':
try:
sub_launch_file = resolve_args(tag.attributes['file'].value, context)
except KeyError as e:
raise RoslaunchDepsException("Cannot load roslaunch <%s> tag: missing required attribute %s.\nXML is %s"%(tag.tagName, str(e), tag.toxml()))
# Check if an empty file is included, and skip if so.
# This will allow a default-empty <include> inside a conditional to pass
if sub_launch_file == '':
if verbose:
print("Empty <include> in %s. Skipping <include> of %s" %
(launch_file, tag.attributes['file'].value))
continue
if verbose:
print("processing included launch %s"%sub_launch_file)
# determine package dependency for included file
sub_pkg = rospkg.get_package_name(os.path.dirname(os.path.abspath(sub_launch_file)))
if sub_pkg is None:
print("ERROR: cannot determine package for [%s]"%sub_launch_file, file=sys.stderr)
if sub_launch_file not in file_deps[launch_file].includes:
file_deps[launch_file].includes.append(sub_launch_file)
if launch_file_pkg != sub_pkg:
file_deps[launch_file].pkgs.append(sub_pkg)
# recurse
file_deps[sub_launch_file] = RoslaunchDeps()
try:
dom = parse(sub_launch_file).getElementsByTagName('launch')
if not len(dom):
print("ERROR: %s is not a valid roslaunch file"%sub_launch_file, file=sys.stderr)
else:
launch_tag = dom[0]
sub_context = _parse_subcontext(tag.childNodes, context)
_parse_launch(launch_tag.childNodes, sub_launch_file, file_deps, verbose, sub_context)
except IOError as e:
raise RoslaunchDepsException("Cannot load roslaunch include '%s' in '%s'"%(sub_launch_file, launch_file))
elif tag.tagName in ['node', 'test']:
try:
pkg, type = [resolve_args(tag.attributes[a].value, context) for a in ['pkg', 'type']]
except KeyError as e:
raise RoslaunchDepsException("Cannot load roslaunch <%s> tag: missing required attribute %s.\nXML is %s"%(tag.tagName, str(e), tag.toxml()))
if (pkg, type) not in file_deps[launch_file].nodes:
file_deps[launch_file].nodes.append((pkg, type))
# we actually want to include the package itself if that's referenced
#if launch_file_pkg != pkg:
if pkg not in file_deps[launch_file].pkgs:
file_deps[launch_file].pkgs.append(pkg)
def test_resolve_args():
from roslaunch.substitution_args import resolve_args, SubstitutionException
r = rospkg.RosPack()
roslaunch_dir = r.get_path('roslaunch')
assert roslaunch_dir
anon_context = {'foo': 'bar'}
arg_context = {'fuga': 'hoge', 'car': 'cdr'}
context = {'anon': anon_context, 'arg': arg_context}
tests = [
('$(find roslaunch)', roslaunch_dir),
('hello$(find roslaunch)', 'hello' + roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$$(find roslaunch )', '$' + roslaunch_dir),
('$( find roslaunch )', roslaunch_dir),
('$(find roslaunch )', roslaunch_dir),
('$(find roslaunch)$(find roslaunch)', roslaunch_dir + roslaunch_dir),
('$(find roslaunch)/foo/bar.xml',
roslaunch_dir + os.sep + 'foo' + os.sep + 'bar.xml'),
(r'$(find roslaunch)\foo\bar.xml $(find roslaunch)\bar.xml',
roslaunch_dir + os.sep + 'foo' + os.sep + 'bar.xml ' + roslaunch_dir +
os.sep + 'bar.xml'),
('$(find roslaunch)\\foo\\bar.xml more/stuff\\here',
roslaunch_dir + os.sep + 'foo' + os.sep + 'bar.xml more/stuff\\here'),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)$(optenv ROS_ROOT)',
os.environ['ROS_ROOT'] + os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
('$(optenv NOT_ROS_ROOT)', ''),
('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),
# #1776
('$(anon foo)', 'bar'),
('$(anon foo)/baz', 'bar/baz'),
('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),
# arg
('$(arg fuga)', 'hoge'),
('$(arg fuga)$(arg fuga)', 'hogehoge'),
('$(arg car)$(arg fuga)', 'cdrhoge'),
('$(arg fuga)hoge', 'hogehoge'),
]
for arg, val in tests:
assert val == resolve_args(arg, context=context)
# more #1776
r = resolve_args('$(anon foo)/bar')
assert '/bar' in r
assert not '$(anon foo)' in r
# test against strings that should not match
noop_tests = [
'$(find roslaunch',
'$find roslaunch',
'',
' ',
'noop',
'find roslaunch',
'env ROS_ROOT',
'$$',
')',
'(',
'()',
None,
]
for t in noop_tests:
assert t == resolve_args(t)
failures = [
'$((find roslaunch))',
'$(find $roslaunch)',
'$(find)',
'$(find roslaunch roslaunch)',
'$(export roslaunch)',
'$(env)',
'$(env ROS_ROOT alternate)',
'$(env NOT_SET)',
'$(optenv)',
'$(anon)',
'$(anon foo bar)',
]
for f in failures:
try:
resolve_args(f)
assert False, "resolve_args(%s) should have failed" % f
except SubstitutionException:
pass
def _parse_launch(tags, launch_file, file_deps, verbose, context):
dir_path = os.path.dirname(os.path.abspath(launch_file))
launch_file_pkg = rospkg.get_package_name(dir_path)
# process group, include, node, and test tags from launch file
for tag in [t for t in tags if t.nodeType == DomNode.ELEMENT_NODE]:
if not _check_ifunless(tag, context):
continue
if tag.tagName == 'group':
#descend group tags as they can contain node tags
_parse_launch(tag.childNodes, launch_file, file_deps, verbose, context)
elif tag.tagName == 'arg':
context['arg'][tag.attributes['name'].value] = _get_arg_value(tag, context)
elif tag.tagName == 'include':
try:
sub_launch_file = resolve_args(tag.attributes['file'].value, context)
except KeyError as e:
raise RoslaunchDepsException("Cannot load roslaunch <%s> tag: missing required attribute %s.\nXML is %s"%(tag.tagName, str(e), tag.toxml()))
# Check if an empty file is included, and skip if so.
# This will allow a default-empty <include> inside a conditional to pass
if sub_launch_file == '':
if verbose:
print("Empty <include> in %s. Skipping <include> of %s" %
(launch_file, tag.attributes['file'].value))
continue
if verbose:
print("processing included launch %s"%sub_launch_file)
# determine package dependency for included file
sub_pkg = rospkg.get_package_name(os.path.dirname(os.path.abspath(sub_launch_file)))
if sub_pkg is None:
print("ERROR: cannot determine package for [%s]"%sub_launch_file, file=sys.stderr)
if sub_launch_file not in file_deps[launch_file].includes:
file_deps[launch_file].includes.append(sub_launch_file)
if launch_file_pkg != sub_pkg:
file_deps[launch_file].pkgs.append(sub_pkg)
# recurse
file_deps[sub_launch_file] = RoslaunchDeps()
try:
dom = parse(sub_launch_file).getElementsByTagName('launch')
if not len(dom):
print("ERROR: %s is not a valid roslaunch file"%sub_launch_file, file=sys.stderr)
else:
launch_tag = dom[0]
sub_context = _parse_subcontext(tag.childNodes, context)
_parse_launch(launch_tag.childNodes, sub_launch_file, file_deps, verbose, sub_context)
except IOError as e:
raise RoslaunchDepsException("Cannot load roslaunch include '%s' in '%s'"%(sub_launch_file, launch_file))
elif tag.tagName in ['node', 'test']:
try:
pkg, type = [resolve_args(tag.attributes[a].value, context) for a in ['pkg', 'type']]
except KeyError as e:
raise RoslaunchDepsException("Cannot load roslaunch <%s> tag: missing required attribute %s.\nXML is %s"%(tag.tagName, str(e), tag.toxml()))
if (pkg, type) not in file_deps[launch_file].nodes:
file_deps[launch_file].nodes.append((pkg, type))
# we actually want to include the package itself if that's referenced
#if launch_file_pkg != pkg:
if pkg not in file_deps[launch_file].pkgs:
file_deps[launch_file].pkgs.append(pkg)
