def on_received_get_robot_state(self, context: Context, data: RobotName): env = self.env speed = env.speed omega = 0.0 # XXX if data == self.robot_name: q = env.cartesian_from_weird(env.cur_pos, env.cur_angle) v = geometry.se2_from_linear_angular([speed, 0], omega) state = MyRobotInfo(pose=q, velocity=v, last_action=env.last_action, wheels_velocities=env.wheelVels) rs = MyRobotState(robot_name=data, t_effective=self.current_time, state=state) else: obj: DuckiebotObj = self.npcs[data] q = env.cartesian_from_weird(obj.pos, obj.angle) # FIXME: how to get velocity? v = geometry.se2_from_linear_angular([0, 0], 0) state = MyRobotInfo(pose=q, velocity=v, last_action=np.array([0, 0]), wheels_velocities=np.array([0, 0])) rs = MyRobotState(robot_name=data, t_effective=self.current_time, state=state) # timing information t = timestamp_from_seconds(self.current_time) ts = TimeSpec(time=t, frame=self.episode_name, clock=context.get_hostname()) timing = TimingInfo(acquired={'state': ts}) context.write('robot_state', rs, timing=timing) #, with_schema=True)
def on_received_get_robot_observations(self, context: Context): # timing information t = timestamp_from_seconds(self.last_observations_time) ts = TimeSpec(time=t, frame=self.episode_name, clock=context.get_hostname()) timing = TimingInfo(acquired={'image': ts}) context.write('robot_observations', self.ro, with_schema=True, timing=timing)
def on_received_next_image(self, context: Context): if self.state.nimages >= self.config.images_per_episode: context.write("no_more_images", None) return H, W = self.config.shape values = (128 + np.random.randn(H, W, 3) * 60).astype("uint8") jpg_data = bgr2jpg(values) image = JPGImage(jpg_data) delta = 0.15 t = self.state.nimages * delta time = timestamp_from_seconds(t) ts = TimeSpec(time=time, frame=self.state.episode_name, clock=context.get_hostname()) acquired = {"image": ts} timing = TimingInfo(acquired=acquired) context.write(topic="image", data=image, timing=timing)