def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.is_shutdown = False
self.rectify = False
self.rate = 20
self.bridge = CvBridge()
self.frame_id = rospy.get_namespace().strip('/') + "/camera_optical_frame"
self.camera = UsbCamera(rate=self.rate,callback = self.PublishRaw)
self.cv_image = None
# self.r = rospy.Rate(self.rate)
self.rector = ImageRector()
self.cali_file = os.path.dirname(os.path.abspath(__file__)) + "/default.yaml"
self.camera_info_msg = load_camera_info_2(self.cali_file)
self.camera_info_msg_rect = load_camera_info_2(self.cali_file)
self.image_msg = None # Image()
self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1)
self.pub_rect = rospy.Publisher("~image_rect", Image, queue_size=1)
self.pub_camera_info = rospy.Publisher("~camera_info", CameraInfo, queue_size=1)
# self.pub_compressed = rospy.Publisher("~image_raw/compressed", CompressedImage, queue_size=1)
# self.pub_camera_info_rect = rospy.Publisher("~rect/camera_info", CameraInfo, queue_size=1)
rospy.Service('~camera_set_enable', SetInt32, self.srvCameraSetEnable)
rospy.loginfo("[%s] Initialized......" % (self.node_name))
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.is_shutdown = False
self.DIM = (640, 480)
self.bridge = CvBridge()
self.cv_image = None
# allow the camera to warmup
time.sleep(0.1)
self.rector = ImageRector()
self.detector = ApriltagDetector()
self.visualization = True
self.tag_detect_rate = 4
self.cali_file = rospkg.RosPack().get_path('pi_cam') + "/camera_info/calibrations/default.yaml"
self.camera_info_msg = load_camera_info_2(self.cali_file)
self.image_msg = None
self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1)
self.pub_rect = rospy.Publisher("~image_rect", Image, queue_size=1)
self.pub_detections = rospy.Publisher("~image_detections", Image, queue_size=1)
# self.sub_compressed = rospy.Subscriber("~image_raw", Image, self.cbImg , queue_size=1)
self.sub_compressed = rospy.Subscriber("~image_raw/compressed", CompressedImage, self.cbImg , queue_size=1)
self.tag_srv = rospy.Service('~detect_apriltag', GetApriltagDetections, self.cbGetApriltagDetections)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.is_shutdown = False
self.DIM = (640, 480)
self.rate = 20
self.bridge = CvBridge()
self.frame_id = rospy.get_namespace().strip(
'/') + "/camera_optical_frame"
# initialize the camera and grab a reference to the raw camera capture
self.camera = PiCamera()
self.camera.resolution = self.DIM
self.camera.framerate = self.rate
self.camera.rotation = 180
self.cv_image = None
# allow the camera to warmup
time.sleep(0.1)
self.r = rospy.Rate(self.rate)
self.rector = ImageRector()
self.detector = ApriltagDetector()
self.visualization = True
self.tag_detect_rate = 4
self.cali_file = rospkg.RosPack().get_path(
'pi_cam') + "/camera_info/calibrations/default.yaml"
self.camera_info_msg = load_camera_info_2(self.cali_file)
self.camera_info_msg_rect = load_camera_info_2(self.cali_file)
self.image_msg = Image()
self.rawCapture = PiRGBArray(self.camera, size=self.DIM)
self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1)
self.pub_camera_info = rospy.Publisher("~camera_info",
CameraInfo,
queue_size=1)
self.pub_detections = rospy.Publisher("~image_detections",
Image,
queue_size=1)
self.stream = io.BytesIO()
self.pub_compressed = rospy.Publisher("~image_raw/compressed",
CompressedImage,
queue_size=1)
self.pub_rect = rospy.Publisher("~rect/image", Image, queue_size=1)
self.pub_camera_info_rect = rospy.Publisher("~rect/camera_info",
CameraInfo,
queue_size=1)
self.tag_srv = rospy.Service('~detect_apriltag', GetApriltagDetections,
self.cbGetApriltagDetections)
self.timer_init = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.tag_detect_rate),
self.publish_rect)
def __init__(self):
super(DemoCar, self).__init__()
self.image = None
self.active = True
self.rector = ImageRector()
self.car = CarDriver()
self.captured = 0
self.processed = 0
self.speed = 0.8
self.random_dir = -1
def test_cam():
demo_car = DemoCar()
car.setWheelsSpeed(0, 0)
start = time.time()
capture = cv2.VideoCapture(0)
rector = ImageRector()
while True:
ret, image = capture.read()
if ret == False:
break
# 根据阈值找到对应颜色
image = rector.rect(image)
image = cv2.resize(image, (320, 240))
image = cv2.flip(image, -1)
end = time.time()
print('last update: %.2f ms' % ((end - start) * 1000))
start = end
cv2.imshow("images", image)
# cv2.imshow("images", np.hstack([image, output]))
c = cv2.waitKey(2)
if c == 27:
break
class ApriltagDetectorNode(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.is_shutdown = False
self.DIM = (640, 480)
self.bridge = CvBridge()
self.cv_image = None
# allow the camera to warmup
time.sleep(0.1)
self.rector = ImageRector()
self.detector = ApriltagDetector()
self.visualization = True
self.tag_detect_rate = 4
self.cali_file = rospkg.RosPack().get_path('pi_cam') + "/camera_info/calibrations/default.yaml"
self.camera_info_msg = load_camera_info_2(self.cali_file)
self.image_msg = None
self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1)
self.pub_rect = rospy.Publisher("~image_rect", Image, queue_size=1)
self.pub_detections = rospy.Publisher("~image_detections", Image, queue_size=1)
# self.sub_compressed = rospy.Subscriber("~image_raw", Image, self.cbImg , queue_size=1)
self.sub_compressed = rospy.Subscriber("~image_raw/compressed", CompressedImage, self.cbImg , queue_size=1)
self.tag_srv = rospy.Service('~detect_apriltag', GetApriltagDetections, self.cbGetApriltagDetections)
def cbImg(self,image_msg):
self.image_msg = image_msg
def cbGetApriltagDetections(self,params):
# print(params)
if self.image_msg == None:
return GetApriltagDetectionsResponse()
if hasattr(self.image_msg,'format'): # CompressedImage
try:
cv_image = bgr_from_jpg(self.image_msg.data)
except ValueError as e:
rospy.loginfo('Anti_instagram cannot decode image: %s' % e)
return
else: # Image
cv_image = self.bridge.imgmsg_to_cv2(self.image_msg, desired_encoding="bgr8")
self.pub_raw.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
# rect_image = cv_image
rect_image = self.rector.rect(cv_image)
self.pub_rect.publish(self.bridge.cv2_to_imgmsg(rect_image,"bgr8"))
tags = self.detector.detect(rect_image)
if self.visualization:
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(rect_image, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0))
# cv2.putText(rect_image, "中文测试", # not work
cv2.putText(rect_image, str(tag.tag_id),
org=(tag.corners[0, 0].astype(int)+10,tag.corners[0, 1].astype(int)+10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
imgmsg = self.bridge.cv2_to_imgmsg(rect_image,"bgr8")
self.pub_detections.publish(imgmsg)
return self.toApriltagDetections(tags)
def toApriltagDetections(self,tags):
msg = GetApriltagDetectionsResponse()
for tag in tags:
r = R.from_dcm(np.array(tag.pose_R))
offset = np.array(tag.pose_t)*100
euler = r.as_euler('xyz', degrees=True)
detection = ApriltagPose(id=tag.tag_id,pose_r=euler,pose_t=offset)
msg.detections.append(detection)
return msg
def onShutdown(self):
rospy.loginfo("[%s] Closing camera." % (self.node_name))
self.is_shutdown = True
rospy.loginfo("[%s] Shutdown." % (self.node_name))
class DemoCar(object):
"""docstring for DemoCar"""
def __init__(self):
super(DemoCar, self).__init__()
self.image = None
self.active = True
self.rector = ImageRector()
self.car = CarDriver()
self.captured = 0
self.processed = 0
self.speed = 0.8
self.random_dir = -1
# thread.start_new_thread(camera_node.startCaptureCompressed, ())
def start_capture(self, rate=30):
self.capture = cv2.VideoCapture(0)
self.capture.set(cv2.CAP_PROP_FPS, 30)
self.last_update = time.time()
print('capture start')
self.capture_time = time.time()
while self.active:
ret, self.image = self.capture.read()
end = time.time()
self.captured = self.captured + 1
self.last_update = end
if self.captured % 150 == 0:
# print('captured %d frame in %.2f s' % (150,(time.time() - self.capture_time)))
self.capture_time = time.time()
# print('capture last update: %.2f ms' % ((end-self.last_update)*1000))
time.sleep(0.004)
print('capture end')
self.capture.release()
def follow_yellow(self):
lost_count = 0
start = time.time()
while self.image is None:
print('capture not start')
time.sleep(1)
# 根据阈值找到对应颜色
last_update = self.last_update
self.process_time = time.time()
while True:
if self.last_update == last_update:
print('image not update , skip')
time.sleep(0.005)
continue
# print('run last update %.2f ms' % (time.time() - self.last_update))
self.processed = self.processed + 1
if self.processed % 150 == 0:
print('processed %d frame in %.2f s' %
(150, (time.time() - self.process_time)))
self.process_time = time.time()
image = self.image
image = self.rector.rect(image)
image = cv2.resize(image, (320, 240))
image_raw = cv2.flip(image, -1)
image = image_raw[120:140, :]
# cnt_r = detect_cnt(image,[])
cnt_y = detect_cnt(image, yellow_hsv)
if cnt_y is not None:
# print('yellow')
# y_center,y_wh,y_angle = cv2.minAreaRect(cnt_y)
# print(y_center,y_wh,y_angle)
x, y, w, h = cv2.boundingRect(cnt_y)
y_center = [x + w / 2, y + h / 2]
cv2.drawContours(image, [cnt_y],
-1, (0, 255, 255),
thickness=2)
offset = 70 - y_center[0]
bias = offset / 160.0
self.speed = 1.0 * (1 - bias)
if self.speed < 0.8:
self.speed = 0.8
left = self.speed * (1 - bias)
right = self.speed * (1 + bias)
self.car.setWheelsSpeed(left, right)
lost_count = 0
else:
bias = 0.05 * self.random_dir
left = self.speed * (1 - bias) * 0.5
right = self.speed * (1 + bias) * 0.5
# self.car.setWheelsSpeed(left,right)
self.car.setWheelsSpeed(0, 0)
self.random_dir = self.random_dir * -1
# self.adjust_self()
lost_count = lost_count + 1
if lost_count > 15:
print('yellow line interrupt! at red line')
break
image_raw[120:140, :] = image[:, :]
cv2.imshow("images", image)
# cv2.imshow("images", image_raw)
# cv2.imshow("images", np.hstack([image, output]))
c = cv2.waitKey(2)
if c == 27:
break
def go_ahead_2_seconds(self):
self.car.setWheelsSpeed(0.9, 0.9)
time.sleep(2)
self.car.setWheelsSpeed(0, 0)
def go_ahead_and_stop(self):
self.car.setWheelsSpeed(0.9, 0.9)
time.sleep(1.8)
self.car.setWheelsSpeed(0, 0)
def go_ahead_and_detect_yellow(self):
cnt_y = None
self.car.setWheelsSpeed(0.6, 0.6)
while cnt_y is None:
image = self.image
image = self.rector.rect(image)
image = cv2.resize(image, (320, 240))
image_raw = cv2.flip(image, -1)
image = image_raw[120:140, :]
# cnt_r = detect_cnt(image,[])
cnt_y = detect_cnt(image, yellow_hsv)
self.car.setWheelsSpeed(0.5, 0.5)
self.car.setWheelsSpeed(0.3, 0.3)
def turn_left(self):
self.car.setWheelsSpeed(0.8, 0.8)
time.sleep(2)
self.car.setWheelsSpeed(0, 0.8)
time.sleep(1)
self.car.setWheelsSpeed(0.8, 0.8)
time.sleep(1)
car.setWheelsSpeed(0, 0)
def turn_right(self):
# self.car.setWheelsSpeed(0.5,0.5)
# time.sleep(0.5)
self.car.setWheelsSpeed(0.8, 0.3)
time.sleep(1.5)
car.setWheelsSpeed(0, 0)
def adjust_self(self):
image = self.image
image = cv2.resize(image, (320, 240))
cnt_r = detect_cnt(image, yellow_hsv)
if cnt_r is not None:
print('red')
r_center, r_wh, r_angle = cv2.minAreaRect(cnt_r)
print(r_center, r_wh, r_angle)
class CameraNode(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.is_shutdown = False
self.rectify = False
self.rate = 20
self.bridge = CvBridge()
self.frame_id = rospy.get_namespace().strip('/') + "/camera_optical_frame"
self.camera = UsbCamera(rate=self.rate,callback = self.PublishRaw)
self.cv_image = None
# self.r = rospy.Rate(self.rate)
self.rector = ImageRector()
self.cali_file = os.path.dirname(os.path.abspath(__file__)) + "/default.yaml"
self.camera_info_msg = load_camera_info_2(self.cali_file)
self.camera_info_msg_rect = load_camera_info_2(self.cali_file)
self.image_msg = None # Image()
self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1)
self.pub_rect = rospy.Publisher("~image_rect", Image, queue_size=1)
self.pub_camera_info = rospy.Publisher("~camera_info", CameraInfo, queue_size=1)
# self.pub_compressed = rospy.Publisher("~image_raw/compressed", CompressedImage, queue_size=1)
# self.pub_camera_info_rect = rospy.Publisher("~rect/camera_info", CameraInfo, queue_size=1)
rospy.Service('~camera_set_enable', SetInt32, self.srvCameraSetEnable)
rospy.loginfo("[%s] Initialized......" % (self.node_name))
def srvCameraSetEnable(self,params):
if params.value == 1 and self.camera.active == False:
ret = self.camera.open_camera(params.port)
return SetInt32Response(0,ret)
elif params.value == 0:
self.camera.active = False
return SetInt32Response(0,0)
else:
return SetInt32Response(1,1)
def PublishRaw(self,cv_image):
# Publish raw image
cv_image = cv2.flip(cv_image,-1)
self.cv_image = cv_image
img_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
# time_2 = time.time()
img_msg.header.stamp = rospy.Time.now()
self.camera_info_msg.header = img_msg.header
img_msg.header.frame_id = img_msg.header.frame_id
self.pub_raw.publish(img_msg)
self.pub_camera_info.publish(self.camera_info_msg)
self.image_msg = img_msg
if self.rectify:
rect_image = self.rector.rect(cv_image)
img_msg_rect = self.bridge.cv2_to_imgmsg(rect_image, "bgr8")
img_msg_rect.header = img_msg.header
self.pub_rect.publish(img_msg_rect)
def onShutdown(self):
rospy.loginfo("[%s] Closing camera." % (self.node_name))
self.is_shutdown = True
rospy.loginfo("[%s] Shutdown." % (self.node_name))
class CameraNode(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.is_shutdown = False
self.DIM = (640, 480)
self.rate = 20
self.bridge = CvBridge()
self.frame_id = rospy.get_namespace().strip(
'/') + "/camera_optical_frame"
# initialize the camera and grab a reference to the raw camera capture
self.camera = PiCamera()
self.camera.resolution = self.DIM
self.camera.framerate = self.rate
self.camera.rotation = 180
self.cv_image = None
# allow the camera to warmup
time.sleep(0.1)
self.r = rospy.Rate(self.rate)
self.rector = ImageRector()
self.detector = ApriltagDetector()
self.visualization = True
self.tag_detect_rate = 4
self.cali_file = rospkg.RosPack().get_path(
'pi_cam') + "/camera_info/calibrations/default.yaml"
self.camera_info_msg = load_camera_info_2(self.cali_file)
self.camera_info_msg_rect = load_camera_info_2(self.cali_file)
self.image_msg = Image()
self.rawCapture = PiRGBArray(self.camera, size=self.DIM)
self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1)
self.pub_camera_info = rospy.Publisher("~camera_info",
CameraInfo,
queue_size=1)
self.pub_detections = rospy.Publisher("~image_detections",
Image,
queue_size=1)
self.stream = io.BytesIO()
self.pub_compressed = rospy.Publisher("~image_raw/compressed",
CompressedImage,
queue_size=1)
self.pub_rect = rospy.Publisher("~rect/image", Image, queue_size=1)
self.pub_camera_info_rect = rospy.Publisher("~rect/camera_info",
CameraInfo,
queue_size=1)
self.tag_srv = rospy.Service('~detect_apriltag', GetApriltagDetections,
self.cbGetApriltagDetections)
self.timer_init = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.tag_detect_rate),
self.publish_rect)
# frame = next( self.camera.capture_continuous(self.stream, format="jpeg", use_video_port=True) )
# print(frame)
def startCaptureRawCV(self):
while not self.is_shutdown and not rospy.is_shutdown():
frame = next(
self.camera.capture_continuous(self.rawCapture,
format="bgr",
use_video_port=True))
self.cv_image = frame.array
image_msg = self.bridge.cv2_to_imgmsg(self.cv_image, "bgr8")
image_msg.header.stamp = rospy.Time.now()
image_msg.header.frame_id = self.frame_id
self.image_msg = image_msg
self.pub_raw.publish(image_msg)
self.camera_info_msg.header = image_msg.header
self.pub_camera_info.publish(self.camera_info_msg)
'''
rect_image = self.rector.rect(self.cv_image)
rect_image_msg = self.bridge.cv2_to_imgmsg(rect_image, "bgr8")
rect_image_msg.header = image_msg.header
self.pub_rect.publish(rect_image_msg)
'''
# self.r.sleep()
self.rawCapture.truncate(0)
self.camera.close()
def publish_rect(self, event):
if self.cv_image == None:
return
rect_image = self.rector.rect(self.cv_image)
rect_msg = self.bridge.cv2_to_imgmsg(rect_image, "bgr8")
rect_msg.header.stamp = rospy.Time.now()
self.camera_info_msg_rect.header = rect_msg.header
self.pub_camera_info_rect.publish(self.camera_info_msg_rect)
self.pub_rect.publish(rect_msg)
def startCaptureCompressed(self):
while not self.is_shutdown and not rospy.is_shutdown():
gen = self.genCaptureCompressed()
self.camera.capture_sequence(gen,
format="jpeg",
use_video_port=True,
splitter_port=0)
self.camera.close()
def genCaptureCompressed(self):
while not self.is_shutdown and not rospy.is_shutdown():
yield self.stream
self.stream.seek(0)
image_msg = CompressedImage()
image_msg.format = "jpeg"
image_msg.data = self.stream.getvalue()
image_msg.header.stamp = rospy.Time.now()
image_msg.header.frame_id = self.frame_id
self.pub_compressed.publish(image_msg)
# self.decodeAndPublishRaw(image_msg)
# self.r.sleep()
self.stream.seek(0)
self.stream.truncate()
def decodeAndPublishRaw(self, image_msg):
# Publish raw image
np_arr = np.fromstring(image_msg.data, np.uint8)
cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# time_1 = time.time()
img_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
# time_2 = time.time()
img_msg.header.stamp = image_msg.header.stamp
img_msg.header.frame_id = image_msg.header.frame_id
self.pub_raw.publish(img_msg)
def cbGetApriltagDetections(self, params):
# print(params)
if self.cv_image != None:
rect_image = self.rector.rect(self.cv_image)
# self.pub_rect.publish(self.bridge.cv2_to_imgmsg(rect_image,"bgr8"))
tags = self.detector.detect(rect_image)
if self.visualization:
for tag in tags:
for idx in range(len(tag.corners)):
cv2.line(rect_image,
tuple(tag.corners[idx - 1, :].astype(int)),
tuple(tag.corners[idx, :].astype(int)),
(0, 255, 0))
# cv2.putText(rect_image, "中文测试", # not work
cv2.putText(rect_image,
str(tag.tag_id),
org=(tag.corners[0, 0].astype(int) + 10,
tag.corners[0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.8,
color=(0, 0, 255))
imgmsg = self.bridge.cv2_to_imgmsg(rect_image, "bgr8")
self.pub_detections.publish(imgmsg)
return self.toApriltagDetections(tags)
return GetApriltagDetectionsResponse()
def toApriltagDetections(self, tags):
msg = GetApriltagDetectionsResponse()
for tag in tags:
r = R.from_dcm(np.array(tag.pose_R))
offset = np.array(tag.pose_t) * 100
euler = r.as_euler('xyz', degrees=True)
detection = ApriltagPose(id=tag.tag_id,
pose_r=euler,
pose_t=offset)
msg.detections.append(detection)
return msg
def onShutdown(self):
rospy.loginfo("[%s] Closing camera." % (self.node_name))
self.is_shutdown = True
rospy.loginfo("[%s] Shutdown." % (self.node_name))