def test_substitution_args_arg(self):
set_substitution_args_context(load_mappings(['sub_arg:=my_arg']))
self.assertTrue(
xml_matches(
quick_xacro('''<a><f v="$(arg sub_arg)" /></a>'''),
'''<a><f v="my_arg" /></a>'''))
set_substitution_args_context({})
def reload_mappings(argv):
"""
Re-initialize the name remapping table.
@param argv: Command line arguments to this program. ROS reads
these arguments to find renaming params.
@type argv: [str]
"""
global _mappings
_mappings = load_mappings(argv)
def reload_mappings(argv):
"""
Re-initialize the name remapping table.
@param argv: Command line arguments to this program. ROS reads
these arguments to find renaming params.
@type argv: [str]
"""
global _mappings
_mappings = load_mappings(argv)
def process_args(argv, require_input=True):
parser = ColoredOptionParser(usage="usage: %prog [options] <input>",
formatter=IndentedHelpFormatterWithNL())
parser.add_option("-o", dest="output", metavar="FILE",
help="write output to FILE instead of stdout")
parser.add_option("--oldorder", action="store_false", dest="in_order",
help="use traditional processing order [deprecated default]")
parser.add_option("--inorder", "-i", action="store_true", dest="in_order",
help="use processing in read order")
parser.add_option("--check-order", action="store_true", dest="do_check_order",
help="check document for inorder processing", default=False)
parser.add_option("--deps", action="store_true", dest="just_deps",
help="print file dependencies")
parser.add_option("--includes", action="store_true", dest="just_includes",
help="only process includes")
parser.add_option("--xacro-ns", action="store_false", default=True, dest="xacro_ns",
help="require xacro namespace prefix for xacro tags")
# verbosity options
parser.add_option("-q", action="store_const", dest="verbosity", const=0,
help="quiet operation suppressing warnings")
parser.add_option("-v", action="count", dest="verbosity",
help="increase verbosity")
parser.add_option("--verbosity", metavar='level', dest="verbosity", type='int',
help=textwrap.dedent("""\
set verbosity level
0: quiet, suppressing warnings
1: default, showing warnings and error locations
2: show stack trace
3: log property definitions and usage on top level
4: log property definitions and usage on all levels"""))
# process substitution args
mappings = load_mappings(argv)
parser.set_defaults(in_order=False, just_deps=False, just_includes=False,
verbosity=1)
filtered_args = [a for a in argv if REMAP not in a] # filter-out REMAP args
(options, pos_args) = parser.parse_args(filtered_args)
if options.in_order and options.just_includes:
parser.error("options --inorder and --includes are mutually exclusive")
if options.do_check_order:
options.in_order = True # check-order implies inorder
if len(pos_args) != 1:
if require_input:
parser.error("expected exactly one input file as argument")
else:
pos_args = [None]
options.mappings = mappings
return options, pos_args[0]
def load_sysargs_into_context(context, argv):
"""
Load in ROS remapping arguments as arg assignments for context.
@param context: context to load into. context's resolve_dict for 'arg' will be reinitialized with values.
@type context: L{LoaderContext{
@param argv: command-line arguments
@type argv: [str]
"""
# we use same command-line spec as ROS nodes
mappings = load_mappings(argv)
context.resolve_dict['arg'] = mappings
def load_sysargs_into_context(context, argv):
"""
Load in ROS remapping arguments as arg assignments for context.
@param context: context to load into. context's resolve_dict for 'arg' will be reinitialized with values.
@type context: L{LoaderContext{
@param argv: command-line arguments
@type argv: [str]
"""
# we use same command-line spec as ROS nodes
mappings = load_mappings(argv)
context.resolve_dict['arg'] = mappings
def test_default_arg_override(self):
set_substitution_args_context(load_mappings(['foo:=4']))
self.assertTrue(
xml_matches(
quick_xacro('''\
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="foo" default="2"/>
<link name="my_link">
<origin xyz="0 0 $(arg foo)"/>
</link>
</robot>
'''), '''\
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="my_link">
<origin xyz="0 0 4"/>
</link>
</robot>'''))
set_substitution_args_context({})
def process_cli_args(argv, require_input=True):
args = {}
parser = OptionParser(usage="usage: %prog [options] <input>")
parser.add_option("-o",
dest="output",
metavar="FILE",
help="write output to FILE instead of stdout")
parser.add_option("--inorder",
action="store_true",
dest="in_order",
help="evaluate document in read order")
parser.add_option("--deps",
action="store_true",
dest="just_deps",
help="print file dependencies")
parser.add_option("--includes",
action="store_true",
dest="just_includes",
help="only process includes")
parser.add_option("--debug",
action="store_true",
dest="debug",
help="print stack trace on exceptions")
# process substitution args
mappings = load_mappings(argv)
parser.set_defaults(in_order=False, just_deps=False, just_includes=False)
(options, pos_args) = parser.parse_args(rospy.myargv(argv))
if len(pos_args) != 1:
if require_input:
parser.error("expected exactly one input file as argument")
else:
pos_args = [None]
options.mappings = mappings
return options, pos_args[0]
def process_args(argv, require_input=True):
parser = ColoredOptionParser(usage="usage: %prog [options] <input>",
formatter=IndentedHelpFormatterWithNL())
parser.add_option("-o",
dest="output",
metavar="FILE",
help="write output to FILE instead of stdout")
parser.add_option(
"--oldorder",
action="store_false",
dest="in_order",
help="use traditional processing order [deprecated default]")
parser.add_option("--inorder",
action="store_true",
dest="in_order",
help="use processing in read order")
parser.add_option("--check-order",
action="store_true",
dest="do_check_order",
help="check document for inorder processing",
default=False)
parser.add_option("--deps",
action="store_true",
dest="just_deps",
help="print file dependencies")
parser.add_option("--includes",
action="store_true",
dest="just_includes",
help="only process includes")
parser.add_option("--xacro-ns",
action="store_false",
default=True,
dest="xacro_ns",
help="require xacro namespace prefix for xacro tags")
# verbosity options
parser.add_option("-q",
action="store_const",
dest="verbosity",
const=0,
help="quiet operation suppressing warnings")
parser.add_option("-v",
action="count",
dest="verbosity",
help="increase verbosity")
parser.add_option("--verbosity",
metavar='level',
dest="verbosity",
type='int',
help=textwrap.dedent("""\
set verbosity level
0: quiet, suppressing warnings
1: default, showing warnings and error locations
2: show stack trace
3: log property definitions and usage on top level
4: log property definitions and usage on all levels"""))
# process substitution args
mappings = load_mappings(argv)
parser.set_defaults(in_order=False,
just_deps=False,
just_includes=False,
verbosity=1)
filtered_args = [a for a in argv
if REMAP not in a] # filter-out REMAP args
(options, pos_args) = parser.parse_args(filtered_args)
if options.in_order and options.just_includes:
parser.error("options --inorder and --includes are mutually exclusive")
if options.do_check_order:
options.in_order = True # check-order implies inorder
if len(pos_args) != 1:
if require_input:
parser.error("expected exactly one input file as argument")
else:
pos_args = [None]
options.mappings = mappings
return options, pos_args[0]
for o, a in opts:
if o == '-h':
print_usage(0)
elif o == '-o':
output = open(a, 'w')
elif o == '--deps':
just_deps = True
elif o == '--includes':
just_includes = True
if len(args) < 1:
print("No input given")
print_usage(2)
# Process substitution args
set_substitution_args_context(load_mappings(sys.argv))
f = open(args[0])
doc = None
try:
doc = parse(f)
except xml.parsers.expat.ExpatError:
sys.stderr.write("Expat parsing error. Check that:\n")
sys.stderr.write(" - Your XML is correctly formed\n")
sys.stderr.write(" - You have the xacro xmlns declaration: " +
"xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n")
sys.stderr.write("\n")
raise
finally:
f.close()
def process_args(argv, require_input=True):
parser = ColoredOptionParser(usage="usage: %prog [options] <input>",
formatter=IndentedHelpFormatterWithNL())
parser.add_option("-o",
dest="output",
metavar="FILE",
help="write output to FILE instead of stdout")
parser.add_option("--inorder",
"-i",
action="store_true",
dest="in_order",
help="use processing in read order [default]")
parser.add_option("--legacy",
action="store_false",
dest="in_order",
help="use legacy processing order [deprecated]")
parser.add_option("--check-order",
action="store_true",
dest="do_check_order",
help="check document for inorder processing",
default=False)
parser.add_option("--deps",
action="store_true",
dest="just_deps",
help="print file dependencies")
parser.add_option("--includes",
action="store_true",
dest="just_includes",
help="only process includes [deprecated]")
parser.add_option("--xacro-ns",
action="store_false",
default=True,
dest="xacro_ns",
help="require xacro namespace prefix for xacro tags")
# verbosity options
parser.add_option("-q",
action="store_const",
dest="verbosity",
const=0,
help="quiet operation suppressing warnings")
parser.add_option("-v",
action="count",
dest="verbosity",
help="increase verbosity")
parser.add_option("--verbosity",
metavar='level',
dest="verbosity",
type='int',
help=textwrap.dedent("""\
set verbosity level
0: quiet, suppressing warnings
1: default, showing warnings and error locations
2: show stack trace
3: log property definitions and usage on top level
4: log property definitions and usage on all levels"""))
# process substitution args
try:
from rosgraph.names import load_mappings, REMAP
mappings = load_mappings(argv)
filtered_args = [a for a in argv
if REMAP not in a] # filter-out REMAP args
except ImportError as e:
warning(e)
mappings = {}
filtered_args = argv
parser.set_defaults(just_deps=False, just_includes=False, verbosity=1)
(options, pos_args) = parser.parse_args(filtered_args)
if options.in_order is None:
# --inorder is default, but it's incompatible to --includes
options.in_order = not options.just_includes
elif options.in_order == True:
message(
"xacro: in-order processing became default in ROS Melodic. You can drop the option."
)
if options.in_order == False:
warning(
"xacro: Legacy processing is deprecated since ROS Jade and will be removed in N-turtle."
)
message(
"To check for compatibility of your document, use option --check-order.",
color='yellow')
message(
"For more infos, see http://wiki.ros.org/xacro#Processing_Order",
color='yellow')
if options.just_includes:
warning("xacro: option --includes is deprecated")
# --inorder is incompatible to --includes: --inorder processing starts evaluation
# while --includes should return the unmodified document
if options.in_order and options.just_includes:
parser.error("options --inorder and --includes are mutually exclusive")
if options.do_check_order:
options.in_order = True # check-order implies inorder
if len(pos_args) != 1:
if require_input:
parser.error("expected exactly one input file as argument")
else:
pos_args = [None]
options.mappings = mappings
return options, pos_args[0]
sys.stderr.write(
"Could not find %s. Have you created the file?\nError: %s\n" %
(robot_yaml_file, str(e)))
sys.exit(3)
try:
if robot_xacro is None:
# if no xacro file is provided, use the default one
package_path = rospkg.RosPack().get_path('nifti_robot_description')
robot_xacro = os.path.join(package_path, 'urdf',
'nifti_robot.xacro')
if len(args) > 0:
# if other arguments are provided, treat them as ROS key:=value stuff and give them precedence over what is
# on the parameter server
mappings = load_mappings(args)
parts_config.update(mappings)
try:
doc = xacro.process_file(robot_xacro,
mappings=dict([
(key, str(val))
for (key,
val) in parts_config.iteritems()
]),
in_order=True,
xacro_ns=False)
print doc.toprettyxml()
except Exception, e:
sys.stderr.write("Error converting XACRO to URDF: %s, %r\n" %
(str(e), e))
if not name or name == SEP:
return name
elif name[0] == SEP:
return '/' + '/'.join([x for x in name.split(SEP) if x])
else:
return '/'.join([x for x in name.split(SEP) if x])
##if len(name) > 1 and name[-1] == SEP:
## return name[:-1]
##return name
# Mappings override name resolution by substituting fully-qualified
# names in for local name references. They override any name
# reference, with exception of '.local' names. We load remapping args
# as soon as client API is referenced so that they are initialized
# before Topic constructors are invoked.
_mappings = load_mappings(sys.argv)
_resolved_mappings = {}
def reload_mappings(argv):
"""
Re-initialize the name remapping table.
@param argv: Command line arguments to this program. ROS reads
these arguments to find renaming params.
@type argv: [str]
"""
global _mappings
_mappings = load_mappings(argv)
# #1810
def initialize_mappings(node_name):
def test_load_mappings():
from rosgraph.names import load_mappings
assert {} == load_mappings([])
assert {} == load_mappings(['foo'])
assert {} == load_mappings([':='])
assert {} == load_mappings([':=:='])
assert {} == load_mappings(['f:='])
assert {} == load_mappings([':=b'])
assert {} == load_mappings(['foo:=bar:=baz'])
# should ignore node param assignments
assert {} == load_mappings(['_foo:=bar'])
assert {'foo': 'bar'} == load_mappings(['foo:=bar'])
# should allow double-underscore names
assert {'__foo': 'bar'} == load_mappings(['__foo:=bar'])
assert {'foo': 'bar'} == load_mappings(['./f', '-x', '--blah', 'foo:=bar'])
assert {'a': '1', 'b': '2', 'c': '3'} == load_mappings(['c:=3', 'c:=', ':=3', 'a:=1', 'b:=2'])
def main():
try:
opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:",
['deps', 'includes'])
except getopt.GetoptError as err:
print(str(err))
print_usage(2)
just_deps = False
just_includes = False
output = sys.stdout
for o, a in opts:
if o == '-h':
print_usage(0)
elif o == '-o':
output = open(a, 'w')
elif o == '--deps':
just_deps = True
elif o == '--includes':
just_includes = True
if len(args) < 1:
print("No input given")
print_usage(2)
# Process substitution args
set_substitution_args_context(load_mappings(sys.argv))
f = open(args[0])
doc = None
try:
doc = parse(f)
except xml.parsers.expat.ExpatError:
sys.stderr.write("Expat parsing error. Check that:\n")
sys.stderr.write(" - Your XML is correctly formed\n")
sys.stderr.write(" - You have the xacro xmlns declaration: " +
"xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n")
sys.stderr.write("\n")
raise
finally:
f.close()
process_includes(doc, os.path.dirname(args[0]))
if just_deps:
for inc in all_includes:
sys.stdout.write(inc + " ")
sys.stdout.write("\n")
elif just_includes:
doc.writexml(output)
print()
else:
eval_self_contained(doc)
banner = [
xml.dom.minidom.Comment(c) for c in [
" %s " % ('=' * 83),
" | This document was autogenerated by xacro from %-30s | "
% args[0],
" | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | "
% "",
" %s " % ('=' * 83)
]
]
first = doc.firstChild
for comment in banner:
doc.insertBefore(comment, first)
output.write(doc.toprettyxml(indent=' '))
print()
def test_load_mappings():
from rosgraph.names import load_mappings
assert {} == load_mappings([])
assert {} == load_mappings(['foo'])
assert {} == load_mappings([':='])
assert {} == load_mappings([':=:='])
assert {} == load_mappings(['f:='])
assert {} == load_mappings([':=b'])
assert {} == load_mappings(['foo:=bar:=baz'])
# should ignore node param assignments
assert {} == load_mappings(['_foo:=bar'])
assert {'foo': 'bar'} == load_mappings(['foo:=bar'])
# should allow double-underscore names
assert {'__foo': 'bar'} == load_mappings(['__foo:=bar'])
assert {'foo': 'bar'} == load_mappings(['./f', '-x', '--blah', 'foo:=bar'])
assert {
'a': '1',
'b': '2',
'c': '3'
} == load_mappings(['c:=3', 'c:=', ':=3', 'a:=1', 'b:=2'])
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()
if not name or name == SEP:
return name
elif name[0] == SEP:
return '/' + '/'.join([x for x in name.split(SEP) if x])
else:
return '/'.join([x for x in name.split(SEP) if x])
##if len(name) > 1 and name[-1] == SEP:
## return name[:-1]
##return name
# Mappings override name resolution by substituting fully-qualified
# names in for local name references. They override any name
# reference, with exception of '.local' names. We load remapping args
# as soon as client API is referenced so that they are initialized
# before Topic constructors are invoked.
_mappings = load_mappings(sys.argv)
_resolved_mappings = {}
def reload_mappings(argv):
"""
Re-initialize the name remapping table.
@param argv: Command line arguments to this program. ROS reads
these arguments to find renaming params.
@type argv: [str]
"""
global _mappings
_mappings = load_mappings(argv)
# #1810
def initialize_mappings(node_name):
def main():
try:
opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes'])
except getopt.GetoptError as err:
print(str(err))
print_usage(2)
just_deps = False
just_includes = False
output = sys.stdout
for o, a in opts:
if o == '-h':
print_usage(0)
elif o == '-o':
output = open(a, 'w')
elif o == '--deps':
just_deps = True
elif o == '--includes':
just_includes = True
if len(args) < 1:
print("No input given")
print_usage(2)
# Process substitution args
set_substitution_args_context(load_mappings(sys.argv))
f = open(args[0])
doc = None
try:
doc = parse(f)
except xml.parsers.expat.ExpatError:
sys.stderr.write("Expat parsing error. Check that:\n")
sys.stderr.write(" - Your XML is correctly formed\n")
sys.stderr.write(" - You have the xacro xmlns declaration: " +
"xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n")
sys.stderr.write("\n")
raise
finally:
f.close()
process_includes(doc, os.path.dirname(sys.argv[1]))
if just_deps:
for inc in all_includes:
sys.stdout.write(inc + " ")
sys.stdout.write("\n")
elif just_includes:
doc.writexml(output)
print
else:
eval_self_contained(doc)
banner = [xml.dom.minidom.Comment(c) for c in
[" %s " % ('=' * 83),
" | This document was autogenerated by xacro from %-30s | " % args[0],
" | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "",
" %s " % ('=' * 83)]]
first = doc.firstChild
for comment in banner:
doc.insertBefore(comment, first)
output.write(doc.toprettyxml(indent=' '))
#doc.writexml(output, newl = "\n")
print
def process_args(argv, require_input=True):
parser = ColoredOptionParser(usage="usage: %prog [options] <input>",
formatter=IndentedHelpFormatterWithNL())
parser.add_option("-o",
dest="output",
metavar="FILE",
help="write output to FILE instead of stdout")
parser.add_option("--deps",
action="store_true",
dest="just_deps",
help="print file dependencies")
parser.add_option(
"--inorder",
"-i",
action="store_true",
dest="in_order",
help="processing in read order (default, can be omitted)")
# verbosity options
parser.add_option("-q",
action="store_const",
dest="verbosity",
const=0,
help="quiet operation, suppressing warnings")
parser.add_option("-v",
action="count",
dest="verbosity",
help="increase verbosity")
parser.add_option("--verbosity",
metavar='level',
dest="verbosity",
type='int',
help=textwrap.dedent("""\
set verbosity level
0: quiet, suppressing warnings
1: default, showing warnings and error locations
2: show stack trace
3: log property definitions and usage on top level
4: log property definitions and usage on all levels"""))
# process substitution args
try:
from rosgraph.names import load_mappings, REMAP
mappings = load_mappings(argv)
filtered_args = [a for a in argv
if REMAP not in a] # filter-out REMAP args
except ImportError as e:
warning(e)
mappings = {}
filtered_args = argv
parser.set_defaults(just_deps=False, just_includes=False, verbosity=1)
(options, pos_args) = parser.parse_args(filtered_args)
if options.in_order:
message(
"xacro: in-order processing became default in ROS Melodic. You can drop the option."
)
options.in_order = True
if len(pos_args) != 1:
if require_input:
parser.error("expected exactly one input file as argument")
else:
pos_args = [None]
options.mappings = mappings
return options, pos_args[0]