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)))
def __init__(self): self.image_pub_marked = rospy.Publisher( "/assignment6/image_marked_rect", Image, queue_size=1, latch=True) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_undistorted", Image, self.callback, queue_size=10) self.detector = BalloonDetector()
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 __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("/assignment6/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)
class ImageHandler: def __init__(self): self.image_pub_marked = rospy.Publisher("/assignment6/image_marked_rect", Image, queue_size=1, latch=True) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_undistorted", Image, self.callback, queue_size=10) self.detector = BalloonDetector() def callback(self, data): img = self.bridge.imgmsg_to_cv2(data, "bgr8") self.detector.detect_balloons(img) print(self.detector.balloon_positions) self.image_pub_marked.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
def main(): global bridge global detector global pub_steering global pub_speed global force_map global odom_pub shell_script = "sshpass -p 'elfmeter' ssh [email protected] 'v4l2-ctl --device=/dev/usb_cam --set-ctrl exposure_auto=1; v4l2-ctl --device=/dev/usb_cam --set-ctrl exposure_absolute=5'" os.system(shell_script) force_map = np.load("matrixDynamic_lane2.npy") rospy.init_node('my_tenth_node', anonymous=True) bridge = CvBridge() detector = BalloonDetector() pub_steering = rospy.Publisher("/manual_control/steering", Int16, queue_size=1) pub_speed = rospy.Publisher("/manual_control/speed", Int16, queue_size=100, latch=True) odom_pub = rospy.Publisher("/assignment6/odom", Odometry, queue_size=200) rospy.Subscriber("/usb_cam/image_raw/compressed", CompressedImage, callback, queue_size=1) rospy.spin()
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 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()
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)
class ImageHandler: def __init__(self): self.image_pub_marked = rospy.Publisher( "/assignment6/image_marked_rect", Image, queue_size=1, latch=True) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_undistorted", Image, self.callback, queue_size=10) self.detector = BalloonDetector() def callback(self, data): img = self.bridge.imgmsg_to_cv2(data, "bgr8") self.detector.detect_balloons(img) print(self.detector.balloon_positions) self.image_pub_marked.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
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)))
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 main(): global pub_pos global bridge global detector global odom_pub global odom_now_pub odom_pub = rospy.Publisher("/mcpf_gps", Odometry, queue_size=200) odom_now_pub = rospy.Publisher("/odom_now", Odometry, queue_size=200) rospy.init_node('my_little_nodey', anonymous=True) pub_pos = rospy.Publisher('/mcposearray', PoseArray, queue_size=1) initialize_pose_array() bridge = CvBridge() rospy.Subscriber("/odom", Odometry, odom_callback, queue_size=1) detector = BalloonDetector() rospy.Subscriber("/usb_cam/image_rect_color/compressed", CompressedImage, callback, queue_size=1) rospy.spin()
def __init__(self): self.image_pub_marked = rospy.Publisher("/assignment6/image_marked_rect", Image, queue_size=1, latch=True) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_undistorted", Image, self.callback, queue_size=10) self.detector = BalloonDetector()
import cv2 from balloon_detector import BalloonDetector import numpy as np d = BalloonDetector() img = cv2.imread('img.png') hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) hue = hsv[:, :, 0].astype(np.float) pos_avg = 0 for i in xrange(1000): pos_avg += d.calculate_position(hsv, hue) print('done: %s' % pos_avg)