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)