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'))
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.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 read_all(self):
data_central = DataCentral(self.boot_root)
log_index = data_central.get_log_index()
observations = log_index.read_all_robot_streams(id_robot=self.id_robot)
pairs = pairwise(observations)
i = 0
for bd1, bd2 in pairs:
i += 1
if i % 100 == 1:
print('read %d' % i)
y0 = bd1['observations']
y1 = bd2['observations']
u = bd1['commands']
if self.shape is not None:
y0 = scipy_image_resample(y0, self.shape, order=0)
np.clip(y0, 0, 1, y0)
y1 = scipy_image_resample(y1, self.shape, order=0)
np.clip(y1, 0, 1, y1)
log_item = LogItem(y0=y0, y1=y1, u=u, x0=None)
yield log_item
