def get_observation(self, x):
obs_sample = Sample(meta_data=params, T=2)
### collision
coll_time = self.coll_callback.get()
is_coll = coll_time is not None
obs_sample.set_O([int(is_coll)], t=0, sub_obs='collision')
### camera
image_msgs = self.image_callback.get()
assert(len(image_msgs) > 0)
### keep only those before the collision
if is_coll:
image_msgs_filt = [im for im in image_msgs if im.header.stamp.to_sec() < coll_time.to_sec()]
if len(image_msgs_filt) == 0:
image_msgs_filt = [image_msgs[-1]]
image_msgs = image_msgs_filt
image_msg = image_msgs[-1]
im = AgentBebop2d.process_image(image_msg, self.cv_bridge)
# im = np.zeros((params['O']['camera']['height'], params['O']['camera']['width']), dtype=np.float32)
obs_sample.set_O(im.ravel(), t=0, sub_obs='camera')
### publish image for debugging
self.im_debug_pub.publish(self.cv_bridge.cv2_to_imgmsg((255. * im).astype(np.uint8), 'mono8'))
return obs_sample.get_O(t=0)
def get_observation(self, x):
obs_sample = Sample(meta_data=params, T=2)
if self.sim:
is_coll = self.sim_coll
image_msg = self.sim_image
depth_msg = self.sim_depth
back_image_msg = self.sim_back_image
back_depth_msg = self.sim_back_depth
else:
### collision
coll_time = self.coll_callback.get()
is_coll = coll_time is not None
### camera
image_msgs = self.image_callback.get()
assert (len(image_msgs) > 0)
### keep only those before the collision
if is_coll:
image_msgs_filt = [
im for im in image_msgs
if im.header.stamp.to_sec() < coll_time.to_sec()
]
if len(image_msgs_filt) == 0:
image_msgs_filt = [image_msgs[-1]]
image_msgs = image_msgs_filt
image_msg = image_msgs[-1]
if params['O'].get('use_depth', False):
im = AgentRCcar.process_depth(depth_msg, self.cv_bridge)
back_im = AgentRCcar.process_depth(back_depth_msg, self.cv_bridge)
else:
im = AgentRCcar.process_image(image_msg, self.cv_bridge)
back_im = AgentRCcar.process_image(back_image_msg, self.cv_bridge)
ros_image = self.cv_bridge.cv2_to_imgmsg(im, "mono8")
self.pred_image_pub.publish(ros_image)
obs_sample.set_O(im.ravel(), t=0, sub_obs='camera')
obs_sample.set_O(back_im.ravel(), t=0, sub_obs='back_camera')
obs_sample.set_O([int(is_coll)], t=0, sub_obs='collision')
return obs_sample.get_O(t=0)