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)