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)
