def create_launch_xml(agent_ros_node,
robot_ros_node,
agent_node_id='my_agent',
robot_node_id='my_robot',
namespace='boot_olympics',
bag=None,
output=None):
'''
output: None or 'screen'
bag: None or name of the file
'''
check_valid_ros_node_spec(agent_ros_node)
check_valid_ros_node_spec(robot_ros_node)
node_robot = create_ros_node_xml(robot_node_id,
robot_ros_node,
remap={},
output=output)
remap = {
'observations': '%s/observations' % robot_node_id,
'commands': '%s/commands' % robot_node_id
}
node_agent = create_ros_node_xml(agent_node_id, agent_ros_node,
remap=remap, output=output)
if bag is not None:
node_bag = create_ros_node_xml('record', ['rosbag/record', {}],
args="-a -o %s" % bag,
output=output)
else:
node_bag = "<!-- bag disabled -->"
template = """<launch>
<group ns="${namespace}" clear_params="true">
<!-- Robot node -->
${node_robot}
<!-- Agent node -->
${node_agent}
${node_bag}
</group>
</launch>"""
final = Template(template).substitute(
node_robot=indent(node_robot, ' ' * 8),
node_agent=indent(node_agent, ' ' * 8),
namespace=namespace,
node_bag=indent(node_bag, ' ' * 8))
return final
def __init__(self, robot, obs_nuisance=[], cmd_nuisance=[]):
self.inner_robot_name = robot
boot_config = get_boot_config()
id_robot, self.robot = boot_config.robots.instance_smarter(robot)
if not isinstance(self.robot, RobotInterface):
msg = 'Expected RobotInterface, got %s' % describe_type(self.robot)
raise ValueError(msg)
warnings.warn('handle the case better')
self.desc = ('EquivRobot(%s,obs:%s,cmd:%s)'
% (id_robot, obs_nuisance, cmd_nuisance))
# convert to (possibly empty) list of strings
if isinstance(obs_nuisance, str):
obs_nuisance = [obs_nuisance]
if isinstance(cmd_nuisance, str):
cmd_nuisance = [cmd_nuisance]
instance = lambda y: boot_config.nuisances.instance_smarter(y)[1]
self.obs_nuisances = [instance(x) for x in obs_nuisance]
self.cmd_nuisances = [instance(x) for x in cmd_nuisance]
# No - we should not call inverse() before transform_spec()
obs_spec = self.robot.get_spec().get_observations()
for n in self.obs_nuisances:
obs_spec = n.transform_spec(obs_spec)
cmd_spec = self.robot.get_spec().get_commands()
for n in self.cmd_nuisances:
cmd_spec = n.transform_spec(cmd_spec)
# We don't really need to compute this...
try:
self.cmd_nuisances_inv = [x.inverse() for x in self.cmd_nuisances]
# now initialize in reverse
cmd_spec_i = cmd_spec
for n in reversed(self.cmd_nuisances_inv):
cmd_spec_i = n.transform_spec(cmd_spec_i)
StreamSpec.check_same_spec(cmd_spec_i,
self.robot.get_spec().get_commands())
# TODO: why do we do this for commands, not for osbservations?
except Exception as e:
logger.warning('It seems that this chain of nuisances is not '
'exact, but it could be OK to continue. '
' The chain is %s; the error is:\n%s' %
(cmd_nuisance, indent(str(e).strip(), '> ')))
self.spec = BootSpec(obs_spec=obs_spec, cmd_spec=cmd_spec)
self.obs_nuisances_id = obs_nuisance
self.cmd_nuisances_id = cmd_nuisance
def create_ros_node_xml(node_name, ros_node, remap=None, args=None, output=None):
package_name, node_type, params = parse_yaml_ros_node_spec(ros_node)
xml_params = create_params_xml(params)
xml_remap = create_remaps_xml(remap)
template = """<node pkg="${package_name}"
type="${node_type}"
name="${node_name}" ${output} ${args}>
${xml_params}
${xml_remap}
</node>"""
final = Template(template).substitute(
package_name=package_name,
node_name=node_name,
node_type=node_type,
xml_remap=indent(xml_remap, " " * 4),
xml_params=indent(xml_params, " " * 4),
args="args='%s'" % args if args else "",
output="output='%s'" % output if output else "",
)
return final
def check_valid_streamels(streamels):
'''
Raises a ValueError if the data in the structure is not coherent.
This checks, for example, that the default value given respects
the declared upper/lower bounds.
'''
try:
# XXX: how about invalid values?
def check(which, msg, a, b):
if not np.all(which):
error = show_differences(a, b,
is_failure=(~which), condition=msg)
raise ValueError(error)
not_discrete = streamels['kind'] != ValueFormats.Discrete
invalid = streamels['kind'] == ValueFormats.Invalid
check(np.logical_or(invalid,
streamels['lower'] <= streamels['upper']),
'lower<=upper', streamels['lower'], streamels['upper'])
check(np.logical_or(invalid,
streamels['default'] <= streamels['upper']),
'default<=upper', streamels['default'], streamels['upper'])
check(np.logical_or(invalid,
streamels['lower'] <= streamels['default']),
'default<=upper', streamels['lower'], streamels['default'])
check(np.logical_or(not_discrete,
np.round(streamels['default']) == streamels['default']),
'default is discrete',
np.round(streamels['default']),
streamels['default'])
except Exception as e:
msg = ('This streamel specification is not coherent:\n'
'%s\n'
'streamels: %s' %
(indent(str(e), '>'), streamels))
raise ValueError(msg)
def check_conversions(stream_spec1, nuisance):
# print('Checking %s / %s ' % (stream_spec1, nuisance))
nuisance_inv = None
try:
try:
stream_spec2 = nuisance.transform_spec(stream_spec1)
except UnsupportedSpec as e:
logger.info('Skipping %s/%s because incompatible: %s' %
(stream_spec1, nuisance, e))
return
value1 = stream_spec1.get_random_value()
stream_spec1.check_valid_value(value1)
value2 = nuisance.transform_value(value1)
stream_spec2.check_valid_value(value2)
try:
nuisance_inv = nuisance.inverse()
except NuisanceNotInvertible as e:
logger.info('Skipping some tests %s/%s because not invertible:'
' %s' % (stream_spec1, nuisance, e))
return
try:
stream_spec1b = nuisance_inv.transform_spec(stream_spec2)
except UnsupportedSpec as e:
msg = ('The inverse of the nuisance does not seem to be able '
'to handle the result:\n%s\n\n'
' stream_spec1: %s\n'
' nuisance: %s\n'
' stream_spec2: %s\n'
' nuisance_inv: %s\n' %
(indent(str(e), '>'),
stream_spec1.to_yaml(), nuisance,
stream_spec2.to_yaml(), nuisance_inv))
raise ValueError(msg)
try:
StreamSpec.check_same_spec(stream_spec1, stream_spec1b)
except Exception as e:
msg = ('The inverse of the nuisance does not recreate the '
'initial spec:\n%s\n\n'
' stream_spec1: %s\n'
' nuisance: %s\n'
' stream_spec2: %s\n'
' nuisance_inv: %s\n'
' stream_spec1b: %s\n' %
(indent(str(e), '>'),
stream_spec1.to_yaml(), nuisance,
stream_spec2.to_yaml(), nuisance_inv,
stream_spec1b.to_yaml()))
raise ValueError(msg)
value1b = nuisance_inv.transform_value(value2)
stream_spec1.check_valid_value(value1b)
# TODO: if exact
assert_allclose(value1, value1b, rtol=1e-5)
except:
logger.error('Error while testing:')
logger.error(' stream_spec: %s ' % stream_spec1.to_yaml())
logger.error(' nuisance: %s' % nuisance)
logger.error(' nuisance_inv: %s' % nuisance_inv)
raise
