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