def get(self, key):
        if not self.exists(key):
            raise Exception('Could not find key %r.' % key)
        
        filename = self.filename_for_key(key)
        try:
            with warn_long_time(self.warn_long_time, 'reading %r' % key):  
                return safe_pickle_load(filename)

        except Exception as e:
            msg = "Could not unpickle file %r: %s" % (filename, e)
            logger.error(msg)
            raise
예제 #2
0
    def get_servo_commands(self, boot_data):
        self.y = boot_data['observations']

        if self.y_goal is None:
            if os.path.exists(ServoDemo2.state_filename):
                self.info('Loading y_goal from %s' % ServoDemo2.state_filename)
                y_goal = safe_pickle_load(ServoDemo2.state_filename)
                try:
                    obs_spec = self.robot.get_spec().get_observations()
                    obs_spec.check_valid_value(y_goal)

                    self.set_goal_observations(y_goal)
                except Exception as e:
                    self.error(str(e))
                    self.info('Removing file; using current observations')
                    os.unlink(ServoDemo2.state_filename)
                    self.set_goal_observations(self.y)
            else:
                self.set_goal_observations(self.y)

        if self.started_now or self.e0 is None:
            # First iteration with new goal
            self.e0 = self.get_distance_to_goal(self.y)
            self.started_now = False

        self.servo_agent.process_observations(boot_data)

        u = self.servo_agent.choose_commands()

        self.boot_spec.get_commands().check_valid_value(u)

        self.e = self.get_distance_to_goal(self.y)
        if self.state == STATE_SERVOING:
            if self.e < self.error_threshold:
                self.info('stopping here')
                return u * 0


#         warnings.warn('remove')
#         u *= 0.5
#         u[2] *= 0.2
        return u
예제 #3
0
    def get_servo_commands(self, boot_data):
        self.y = boot_data['observations']
        
        if self.y_goal is None:
            if os.path.exists(ServoDemo2.state_filename):
                self.info('Loading y_goal from %s' % ServoDemo2.state_filename)
                y_goal = safe_pickle_load(ServoDemo2.state_filename)
                try: 
                    obs_spec = self.robot.get_spec().get_observations()
                    obs_spec.check_valid_value(y_goal)
                    
                    self.set_goal_observations(y_goal)
                except Exception as e:
                    self.error(str(e))
                    self.info('Removing file; using current observations')
                    os.unlink(ServoDemo2.state_filename)
                    self.set_goal_observations(self.y)
            else:
                self.set_goal_observations(self.y)
            
        if self.started_now or self.e0 is None:
            # First iteration with new goal
            self.e0 = self.get_distance_to_goal(self.y)
            self.started_now = False

        self.servo_agent.process_observations(boot_data)
    
        u = self.servo_agent.choose_commands()         
            
        self.boot_spec.get_commands().check_valid_value(u)

        self.e = self.get_distance_to_goal(self.y)
        if self.state == STATE_SERVOING:    
            if self.e < self.error_threshold:
                self.info('stopping here')
                return u * 0 
    
#         warnings.warn('remove')
#         u *= 0.5
#         u[2] *= 0.2
        return u
예제 #4
0
    def index_file_cached(self, filename, ignore_cache=False):
        cache = '%s.index_cache' % filename
        if os.path.exists(cache) and not ignore_cache:  # TODO: mtime
            try:
                return safe_pickle_load(cache)
            except Exception as e:
                msg = 'Could not unpickle cache %r, deleting.' % friendly_path(cache)
                msg += '\n%s' % e
                logger.warning(msg)
                try:
                    os.unlink(cache)
                except:
                    pass
        logger.debug('Indexing file %r' % friendly_path(filename))
        res = self.index_file(filename)
        for stream in res:
            assert isinstance(stream, BootStream)
            
        logger.debug('Now dumping file %r' % friendly_path(cache))
        with warn_long_time(1, 'dumping %r' % friendly_path(cache)):
            safe_pickle_dump(res, cache, protocol=2)

        return res
    def main(self):
        rospy.init_node('servo_demo', disable_signals=True)
        
        self.info('Started.')
        contracts.disable_all()

        boot_root = rospy.get_param('~boot_root')
        boot_root = expand_environment(boot_root)
        
        config_dir = rospy.get_param('~config_dir')
        id_robot_learned = rospy.get_param('~id_robot_learn')
        
        self.info('loading %r' % config_dir)
        GlobalConfig.global_load_dir(config_dir)
        
        id_agent = rospy.get_param('~id_agent')
        self.id_robot = rospy.get_param('~id_robot')
        self.sleep = rospy.get_param('~sleep', 0.005)
        self.info('sleep: %s' % self.sleep)
#         self.error_threshold = float(rospy.get_param('~error_threshold'))
        
        self.ratio_threshold = float(rospy.get_param('~ratio_threshold'))  # 0.95
        self.d_next_threshold = float(rospy.get_param('~d_next_threshold'))  # 0.03
        self.d_next_threshold_alone = float(rospy.get_param('~d_next_threshold_alone'))  # 0.04
        
        
        nmap = expand_environment(rospy.get_param('~map'))
        self.nmap = safe_pickle_load(nmap)
        
        raise_if_no_state = rospy.get_param('~raise_if_no_state', True)
        
        data_central = DataCentral(boot_root)
        
        ag_st = load_agent_state(data_central, id_agent, id_robot_learned,
                                 reset_state=False,
                                 raise_if_no_state=raise_if_no_state)
        self.agent, state = ag_st
        
        self.info('Loaded state: %s' % state)
        
        self.servo_agent = self.agent.get_servo()
        
        bo_config = get_boot_config()
        self.robot = bo_config.robots.instance(self.id_robot)
        self.boot_spec = self.robot.get_spec()
            
        self.publish_info_init()     
        
        self.y = None
        self.y_goal = None
        self.started_now = False
        self.stopped_now = False
        self.e0 = 1
        self.e = 1         
        self.last_boot_data = None
        self.state = STATE_WAIT
        self.index_cur = None
        
        self.info('Defining services')
        rospy.Service('set_goal', Empty, self.srv_set_goal)
        rospy.Service('start_servo', Empty, self.srv_start_servo)
        rospy.Service('stop_servo', Empty, self.srv_stop_servo)
                
        self.info('Finished initialization') 
        self.count = 0
        
        self.go()
예제 #6
0
    def main(self):
        rospy.init_node('servo_demo', disable_signals=True)

        self.info('Started.')
        contracts.disable_all()

        boot_root = rospy.get_param('~boot_root')
        boot_root = expand_environment(boot_root)

        config_dir = rospy.get_param('~config_dir')
        id_robot_learned = rospy.get_param('~id_robot_learn')

        self.info('loading %r' % config_dir)
        GlobalConfig.global_load_dir(config_dir)

        id_agent = rospy.get_param('~id_agent')
        self.id_robot = rospy.get_param('~id_robot')
        self.sleep = rospy.get_param('~sleep', 0.005)
        self.info('sleep: %s' % self.sleep)
        #         self.error_threshold = float(rospy.get_param('~error_threshold'))

        self.ratio_threshold = float(
            rospy.get_param('~ratio_threshold'))  # 0.95
        self.d_next_threshold = float(
            rospy.get_param('~d_next_threshold'))  # 0.03
        self.d_next_threshold_alone = float(
            rospy.get_param('~d_next_threshold_alone'))  # 0.04

        nmap = expand_environment(rospy.get_param('~map'))
        self.nmap = safe_pickle_load(nmap)

        raise_if_no_state = rospy.get_param('~raise_if_no_state', True)

        data_central = DataCentral(boot_root)

        ag_st = load_agent_state(data_central,
                                 id_agent,
                                 id_robot_learned,
                                 reset_state=False,
                                 raise_if_no_state=raise_if_no_state)
        self.agent, state = ag_st

        self.info('Loaded state: %s' % state)

        self.servo_agent = self.agent.get_servo()

        bo_config = get_boot_config()
        self.robot = bo_config.robots.instance(self.id_robot)
        self.boot_spec = self.robot.get_spec()

        self.publish_info_init()

        self.y = None
        self.y_goal = None
        self.started_now = False
        self.stopped_now = False
        self.e0 = 1
        self.e = 1
        self.last_boot_data = None
        self.state = STATE_WAIT
        self.index_cur = None

        self.info('Defining services')
        rospy.Service('set_goal', Empty, self.srv_set_goal)
        rospy.Service('start_servo', Empty, self.srv_start_servo)
        rospy.Service('stop_servo', Empty, self.srv_stop_servo)

        self.info('Finished initialization')
        self.count = 0

        self.go()