def run_simulation_servo(id_robot, robot, id_agent, agent,
max_observations, max_time,
id_episode, id_environment,
check_valid_values=True):
''' Runs an episode of the simulation. The agent should already been
init()ed. '''
keeper = ObsKeeper(boot_spec=robot.get_spec(), id_robot=id_robot,
check_valid_values=check_valid_values)
obs_spec = robot.get_spec().get_observations()
cmd_spec = robot.get_spec().get_commands()
counter = 0
while counter < max_observations:
obs = robot.get_observations()
assert isinstance(obs, RobotObservations)
if check_valid_values:
obs_spec.check_valid_value(obs.observations)
observations = keeper.push(timestamp=obs.timestamp,
observations=obs.observations,
commands=obs.commands,
commands_source=obs.commands_source,
id_episode=id_episode,
id_world=id_environment)
episode_end = obs.episode_end
yield obs, observations
if observations['time_from_episode_start'] > max_time:
logger.debug('Episode ended at %s for time limit %s > %s ' %
(counter, observations['time_from_episode_start'],
max_time))
break
if episode_end: # Fishy
logger.debug('Episode ended at %s due to obs.episode_end.'
% counter)
break
agent.process_observations(observations)
commands = agent.choose_commands() # repeated
if check_valid_values:
cmd_spec.check_valid_value(commands)
robot.set_commands(commands, id_agent)
counter += 1
def __init__(self, id_robot, robot, maximum_interval=2, sleep=0.1,
check_valid_values=True):
self.robot = robot
self.maximum_interval = maximum_interval
self.sleep = sleep
self.logger = RospyLogger('robot_adapter')
self.publisher = rospy.Publisher('~observations',
BootstrappingObservations,
latch=True)
# Start service
rospy.Service('~commands', BootstrappingCommands,
self.commands_request)
self.episode = self.robot.new_episode()
self.publish = True # XXX
self.keeper = ObsKeeper(boot_spec=robot.get_spec(),
id_robot=id_robot)
self.check_valid_values = check_valid_values
self.obs_spec = self.robot.get_spec().get_observations()
self.cmd_spec = self.robot.get_spec().get_commands()
def write_robot_observations(id_stream, filename, id_robot,
robot, id_episode, id_environment):
logs_format = LogsFormat.get_reader_for(filename)
boot_spec = robot.get_spec()
keeper = ObsKeeper(boot_spec=boot_spec, id_robot=id_robot,
check_valid_values=False)
boot_spec = robot.get_spec()
nvalid = 0
with logs_format.write_stream(filename=filename, id_stream=id_stream,
boot_spec=boot_spec) as writer:
for obs in iterate_robot_observations(robot, sleep=0):
# print('robot_pose: %s' % obs.robot_pose)
# print('got %s' % obs['timestamp'])
boot_observations = keeper.push(timestamp=obs.timestamp,
observations=obs.observations,
commands=obs.commands,
commands_source=obs.commands_source,
id_episode=id_episode,
id_world=id_environment)
extra = {}
warnings.warn('Make this more general')
if obs.robot_pose is not None:
extra['robot_pose'] = obs.robot_pose.tolist()
extra['odom'] = obs.robot_pose.tolist()
writer.push_observations(observations=boot_observations,
extra=extra)
nvalid += 1
if nvalid == 0:
msg = 'No observations could be found in %s' % filename
raise Exception(msg)
def run_simulation_servonav(
id_robot,
robot,
id_agent,
agent,
max_observations,
max_time,
id_episode,
id_environment,
check_valid_values=True,
raise_error_on_collision=True,
):
"""
Runs an episode of the simulation. The agent should already been
init()ed.
yields robot.get_observations(), boot_observatoions
"""
keeper = ObsKeeper(boot_spec=robot.get_spec(), id_robot=id_robot)
obs_spec = robot.get_spec().get_observations()
cmd_spec = robot.get_spec().get_commands()
counter = 0
while True:
obs = robot.get_observations()
if check_valid_values:
obs_spec.check_valid_value(obs.observations)
# print('run_simulation_servonav: obs.obs %s' %
# (str(obs.observations.shape)))
observations = keeper.push(
timestamp=obs.timestamp,
observations=obs.observations,
commands=obs.commands,
commands_source=obs.commands_source,
id_episode=id_episode,
id_world=id_environment,
)
# print('then: obs.obs %s' %
# (str(observations['observations'].shape)))
episode_end = obs.episode_end
yield obs, observations
now = "step %s" % counter
if counter >= max_observations:
logger.info("Finished at %s because %s >= %s" % (now, counter, max_observations))
break
if observations["time_from_episode_start"] > max_time:
logger.info(
"Finished at %s because of max_time: %s > %s" % (now, observations["time_from_episode_start"], max_time)
)
break
if episode_end: # Fishy
msg = "Finished at %s because of robot driver." % now
if raise_error_on_collision:
raise Exception(msg)
else:
logger.info(msg)
break
agent.process_observations(observations)
commands = agent.choose_commands()
if check_valid_values:
cmd_spec.check_valid_value(commands)
robot.set_commands(commands, id_agent)
counter += 1
class ROS2Python():
'''
This class takes converts a BootstrappingObservations ROS structure
into an Observation type object. It also makes some consistency
checks, marks different episodes, discards repeated observations, etc.
The idea is that the data that comes out of this will not need any
other check by the agents.
'''
def __init__(self, spec, max_dt=1, tolerant=False):
self.last = None
self.spec = spec
self.max_dt = max_dt
self.keeper = None
self.tolerant = tolerant
self.commands_spec = self.spec.get_commands()
self.observations_spec = self.spec.get_observations()
def debug(self, s):
# The last time the problem was that bag_read reads
# in timestamp order, not episode order
# logger.debug('ros2python:%s' % s)
pass
def convert(self, ros_obs, filter_doubles=True):
''' Returns None if the same message is repeated. '''
current_data_description = ('[%s#%s at %s]' %
(ros_obs.id_episode, ros_obs.counter, ros_obs.timestamp))
self.debug('Considering %s' % current_data_description)
if self.keeper is None:
self.keeper = ObsKeeper(self.spec, ros_obs.id_robot)
if filter_doubles:
if (self.last is not None and
self.last['counter'] == ros_obs.counter):
self.debug('Removing double for %s' % current_data_description)
return None
# FIXME: this assumes we use floats
y = np.array(ros_obs.sensel_values, dtype='float32')
sensel_values_reshaped = y.reshape(ros_obs.sensel_shape)
commands = np.array(ros_obs.commands)
try:
self.commands_spec.check_valid_value(commands)
self.observations_spec.check_valid_value(sensel_values_reshaped)
except BootInvalidValue as e:
self.debug('Found invalid values: %s' % e)
if self.tolerant:
self.debug('%s: Skipping invalid data.' %
current_data_description)
else:
self.debug('Raising error because not tolerating.')
raise
return None
# obs = Observations()
# obs.time = ros_obs.timestamp
# obs.sensel_values = sensel_values_reshaped
# obs.commands = np.array(ros_obs.commands, dtype='float32')
# obs.commands_source = ros_obs.commands_source
# obs.counter = ros_obs.counter
# obs.id_episode = ros_obs.id_episode
# # obs.dt is useless
# FIXME: we have to do this again :-/
# FIXME: at least, filter the doubles and max_dt
# if self.last_ros_obs is not None:
# obs.dt = obs.time - self.last.time
#
# obs.episode_changed = obs.id_episode != self.last.id_episode
# if not obs.episode_changed:
# #assert obs.dt >= 0
# if obs.dt < 0:
# logger.error('At %s' % current_data_description)
# logger.error('Out of order? previous time: %s '
# 'current: %s dt: %s' %
# (self.last.time, obs.time, obs.dt))
# return None
# if filter_doubles:
# # assert obs.dt > 0
# if obs.dt <= 0:
# logger.error('At %s' % current_data_description)
# logger.error('Strange, should have caught before.')
# return None
#
# if obs.dt > self.max_dt:
# logger.info('Skipping %s due to strange dt %s .' %
# (current_data_description, obs.dt))
# else:
# obs.dt = 0.01 # episode changed
# else:
# obs.dt = 0.01
# obs.episode_changed = True
self.last = self.keeper.push(
timestamp=ros_obs.timestamp,
observations=sensel_values_reshaped,
commands=commands,
commands_source=ros_obs.commands_source,
id_episode=ros_obs.id_episode,
id_world=ros_obs.id_environment)
self.last_ros_obs = ros_obs
return self.last
def convert(self, ros_obs, filter_doubles=True):
''' Returns None if the same message is repeated. '''
current_data_description = ('[%s#%s at %s]' %
(ros_obs.id_episode, ros_obs.counter, ros_obs.timestamp))
self.debug('Considering %s' % current_data_description)
if self.keeper is None:
self.keeper = ObsKeeper(self.spec, ros_obs.id_robot)
if filter_doubles:
if (self.last is not None and
self.last['counter'] == ros_obs.counter):
self.debug('Removing double for %s' % current_data_description)
return None
# FIXME: this assumes we use floats
y = np.array(ros_obs.sensel_values, dtype='float32')
sensel_values_reshaped = y.reshape(ros_obs.sensel_shape)
commands = np.array(ros_obs.commands)
try:
self.commands_spec.check_valid_value(commands)
self.observations_spec.check_valid_value(sensel_values_reshaped)
except BootInvalidValue as e:
self.debug('Found invalid values: %s' % e)
if self.tolerant:
self.debug('%s: Skipping invalid data.' %
current_data_description)
else:
self.debug('Raising error because not tolerating.')
raise
return None
# obs = Observations()
# obs.time = ros_obs.timestamp
# obs.sensel_values = sensel_values_reshaped
# obs.commands = np.array(ros_obs.commands, dtype='float32')
# obs.commands_source = ros_obs.commands_source
# obs.counter = ros_obs.counter
# obs.id_episode = ros_obs.id_episode
# # obs.dt is useless
# FIXME: we have to do this again :-/
# FIXME: at least, filter the doubles and max_dt
# if self.last_ros_obs is not None:
# obs.dt = obs.time - self.last.time
#
# obs.episode_changed = obs.id_episode != self.last.id_episode
# if not obs.episode_changed:
# #assert obs.dt >= 0
# if obs.dt < 0:
# logger.error('At %s' % current_data_description)
# logger.error('Out of order? previous time: %s '
# 'current: %s dt: %s' %
# (self.last.time, obs.time, obs.dt))
# return None
# if filter_doubles:
# # assert obs.dt > 0
# if obs.dt <= 0:
# logger.error('At %s' % current_data_description)
# logger.error('Strange, should have caught before.')
# return None
#
# if obs.dt > self.max_dt:
# logger.info('Skipping %s due to strange dt %s .' %
# (current_data_description, obs.dt))
# else:
# obs.dt = 0.01 # episode changed
# else:
# obs.dt = 0.01
# obs.episode_changed = True
self.last = self.keeper.push(
timestamp=ros_obs.timestamp,
observations=sensel_values_reshaped,
commands=commands,
commands_source=ros_obs.commands_source,
id_episode=ros_obs.id_episode,
id_world=ros_obs.id_environment)
self.last_ros_obs = ros_obs
return self.last
class BootRobotAdapter:
def __init__(self, id_robot, robot, maximum_interval=2, sleep=0.1,
check_valid_values=True):
self.robot = robot
self.maximum_interval = maximum_interval
self.sleep = sleep
self.logger = RospyLogger('robot_adapter')
self.publisher = rospy.Publisher('~observations',
BootstrappingObservations,
latch=True)
# Start service
rospy.Service('~commands', BootstrappingCommands,
self.commands_request)
self.episode = self.robot.new_episode()
self.publish = True # XXX
self.keeper = ObsKeeper(boot_spec=robot.get_spec(),
id_robot=id_robot)
self.check_valid_values = check_valid_values
self.obs_spec = self.robot.get_spec().get_observations()
self.cmd_spec = self.robot.get_spec().get_commands()
def publish_observations(self):
obs = self.robot.get_observations()
if self.check_valid_values:
self.obs_spec.check_valid_value(obs.observations)
self.cmd_spec.check_valid_value(obs.commands)
observations = self.keeper.push(timestamp=obs.timestamp,
observations=obs.observations,
commands=obs.commands,
commands_source=obs.commands_source,
id_episode=self.episode.id_episode,
id_world=self.episode.id_environment)
msg = observations2ros(robot_spec=self.robot.get_spec(),
obs=observations)
self.publisher.publish(msg)
self.logger.info('Published information.')
def commands_request(self, req):
self.logger.info('Received request.')
commands = np.array(req.commands)
sender = req.sender
self.robot.set_commands(commands=commands,
commands_source=sender)
self.publish = True
return BootstrappingCommandsResponse(True)
def loop(self):
self.logger.info('Loop started')
last_observations_sent = 0
while not rospy.is_shutdown():
now = time.time()
publish = False
if self.publish:
self.publish = False
publish = True
if now > last_observations_sent + self.maximum_interval:
last_observations_sent = now
publish = True
if publish:
self.publish_observations()
self.logger.info('Sleeping...')
# self.sleep = 1
rospy.sleep(self.sleep)
