class ImageHandler: def __init__(self, img_pub=False): if img_pub: self.image_pub_marked = rospy.Publisher( "/assignment6/image_marked_ang", Image, queue_size=200, latch=True) else: self.image_pub_marked = None self.odom_pub = rospy.Publisher("/visual_gps/odom", Odometry, queue_size=200) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback, queue_size=1, buff_size=2**24) self.detector = BalloonDetector() pose_covar = PoseWithCovariance(Pose(Point(0, 0, 0), Quaternion()), None) self.odom = Odometry(Header(frame_id='visualgps'), 'base_link', pose_covar, None) def callback(self, data): t_start = rospy.Time.now() img = self.bridge.imgmsg_to_cv2(data, "bgr8") xy = self.detector.calculate_best_position(img) if self.image_pub_marked is not None: self.detector.draw_markers(img) self.image_pub_marked.publish( self.bridge.cv2_to_imgmsg(img, "bgr8")) # Don't publish a pose if location can't be reliably determined if xy is None: print("No location found") return angle = self.detector.calculate_angle() header = self.odom.header header.seq = data.header.seq header.stamp = data.header.stamp pose = self.odom.pose.pose pos = pose.position pos.x, pos.y = xy quaternion = pose.orientation quaternion.z, quaternion.w = math.sin(angle / 2), math.cos(angle / 2) self.odom_pub.publish(self.odom) print('%-30s angle: %6.1f img_age: %5.3f calc: %5.3f' % (xy, np.rad2deg(angle), to_secs(t_start - data.header.stamp), to_secs(rospy.Time.now() - t_start)))
class RangeCalibrator(object): def __init__(self, window='result'): self.detector = BalloonDetector() self.img = None self.window = window cv2.namedWindow(window) cv2.createTrackbar('blur', window, 0, 50, self.set_blur) cv2.setTrackbarPos('blur', window, self.detector.blur_amount) cv2.createTrackbar('min-sat', window, 0, 255, self.set_min_sat) cv2.setTrackbarPos('min-sat', window, self.detector.hsv_range_min[1]) cv2.createTrackbar('min-val', window, 0, 255, self.set_min_val) cv2.setTrackbarPos('min-val', window, self.detector.hsv_range_min[2]) cv2.createTrackbar('min-samples', window, 0, 255, self.set_min_samples) cv2.setTrackbarPos('min-samples', window, self.detector.min_samples) def set_img(self, img): self.img = img self.update() def update(self): if self.img is not None: img = self.img.copy() # avoid overwriting original self.detector.calculate_best_position(img, max_iters=1) cv2.imshow('mask', self.detector.mask) cv2.moveWindow('mask', img.shape[1] + 2, 0) cv2.imshow(self.window, img) def set_blur(self, val): self.detector.blur_amount = val if val % 2 == 1 or val == 0 else val + 1 self.update() def set_min_sat(self, val): self.detector.hsv_range_min[1] = val self.update() def set_min_val(self, val): self.detector.hsv_range_min[2] = val self.update() def set_min_samples(self, val): self.detector.min_samples = val self.update()
class ImageHandler: def __init__(self): self.image_pub_marked = rospy.Publisher("/assignment6/image_marked_ang", Image, queue_size=200, latch=True) self.odom_pub = rospy.Publisher("/assignment6/odom", Odometry, queue_size=200) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_raw/compressed", CompressedImage, self.callback, queue_size=1) self.detector = BalloonDetector() pose_covar = PoseWithCovariance(Pose(Point(0, 0, 0), Quaternion()), None) self.odom = Odometry(Header(frame_id='odom'), 'base_link', pose_covar, None) def callback(self, data): img = self.bridge.compressed_imgmsg_to_cv2(data, "bgr8") xy = self.detector.calculate_best_position(img) for bln in self.detector.balloon_positions: print bln # show detected points in published image self.detector.draw_markers(img) self.image_pub_marked.publish(self.bridge.cv2_to_imgmsg(img, "bgr8")) # Don't publish a pose if location can't be reliably determined if xy is None: print("No location found") return yaw_angle = self.detector.calculate_angle() # publish odometry message header = self.odom.header header.seq = data.header.seq header.stamp = data.header.stamp pose = self.odom.pose.pose pos = pose.position pos.x, pos.y = xy quaternion = pose.orientation quaternion.z, quaternion.w = math.sin(yaw_angle / 2), math.cos(yaw_angle / 2) self.odom_pub.publish(self.odom) # print position and yaw_angle as degrees in the terminal print('{:<30} yaw: {:6.1f}'.format(xy, np.rad2deg(yaw_angle)))