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
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
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
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()
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()
