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))