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)
Exemple #4
0
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)