def on_image(self, ros_data):
# image in
compressed_in = np.fromstring(ros_data.data, np.uint8)
image_in = cv2.imdecode(compressed_in,
cv2.IMREAD_GRAYSCALE) # in [0,255]
self.check_in(image_in)
image_in = ((np.clip(image_in, self.min_input, self.max_input) -
self.min_input) * self.inv_range_input).astype(np.uint8)
image_out = cv2.cvtColor(image_in, cv2.COLOR_GRAY2RGB)
if self.conv is not None and (image_in.max() > 0):
conv = self.conv(image_in.astype(np.float32))
argmax = np.unravel_index(np.argmax(conv), conv.shape)
current = rospy.Time.now()
frozen = (current -
self.last_shift).to_sec() < self.shift_freeze_duration
color = (0, 255, 0)
if frozen:
color = (0, 0, 255)
cv2.circle(image_out, (argmax[1], argmax[0]), 5, color, -1)
if not frozen:
msg = Point()
msg.z = 0
msg.x = argmax[1] * self.img_coef - .5
msg.y = argmax[0] * self.img_coef - .5
self.shift.publish(msg)
if msg.x * msg.x + msg.y * msg.y > self.max_small_shift_radius_2:
self.last_shift = current
# image out
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', image_out)[1]).tostring()
self.pub.publish(msg)
