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.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): 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): # Get path to calibration yaml file self.cali_file = '.' + "/default.yaml" # Load calibration yaml file self.camera_info_msg = load_camera_info_2(self.cali_file) K = np.array(self.camera_info_msg.K).reshape((3, 3)) # D = np.array(self.camera_info_msg.D[:4]) D = np.array([0., 0., 0., 0.]) # P = np.array(self.camera_info_msg.P).reshape((3,4)) DIM = (self.camera_info_msg.width, self.camera_info_msg.height) self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap( K, D, np.eye(3), K, DIM, cv2.CV_16SC2)
def __init__(self): self.node_name = rospy.get_name() self.bridge = CvBridge() self.count = 0 self.verbose = True # Load parameters # TODO cali_file_name is not needed and should be the robot name by default self.cali_file_name = self.setupParam("~cali_file_name", "default") # Setup publisher self.pub_rect = rospy.Publisher("~image_rect", Image, queue_size=1) # Get path to calibration yaml file self.cali_file = rospkg.RosPack().get_path( 'pi_cam' ) + "/camera_info/calibrations/" + self.cali_file_name + ".yaml" if not os.path.isfile(self.cali_file): rospy.logwarn( "[%s] Can't find calibration file: %s.\nUsing default calibration instead." % (self.node_name, self.cali_file)) self.cali_file = rospkg.RosPack().get_path( 'pi_cam') + "/camera_info/calibrations/default.yaml" # Shutdown if no calibration file not found if not os.path.isfile(self.cali_file): rospy.signal_shutdown("Found no calibration file ... aborting") # Print out and prepare message rospy.loginfo("[%s] Using calibration file: %s" % (self.node_name, self.cali_file)) # Load calibration yaml file self.camera_info_msg = load_camera_info_2(self.cali_file) rospy.loginfo("[%s] CameraInfo: %s" % (self.node_name, self.camera_info_msg)) K = np.array(self.camera_info_msg.K).reshape((3, 3)) # D = np.array(self.camera_info_msg.D[:4]) D = np.array([0., 0., 0., 0.]) # P = np.array(self.camera_info_msg.P).reshape((3,4)) DIM = (self.camera_info_msg.width, self.camera_info_msg.height) self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap( K, D, np.eye(3), K, DIM, cv2.CV_16SC2) typemsg = "CompressedImage" rospy.logwarn("[%s] ==============%s", self.node_name, typemsg) self.sub_img_compressed = rospy.Subscriber( "/ubiquityrobot/camera_node/image/compressed", CompressedImage, self.cbCompressedImage, queue_size=1)
def __init__(self): self.node_name = rospy.get_name() 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) rospy.loginfo("[%s] CameraInfo: %s" % (self.node_name, self.camera_info_msg)) K = np.array(self.camera_info_msg.K).reshape((3, 3)) D = np.array(self.camera_info_msg.D) D = np.array([0., 0., 0., 0.]) # P = np.array(self.camera_info_msg.P).reshape((3,4)) DIM = (self.camera_info_msg.width, self.camera_info_msg.height) self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap( K, D, np.eye(3), K, DIM, cv2.CV_16SC2)
def __init__(self): self.node_name = rospy.get_name() # Load parameters self.config = self.setupParam("~config", "baseline") # TODO cali_file_name is not needed and should be the robot name by default self.cali_file_name = self.setupParam("~cali_file_name", "default") self.image_type = self.setupParam("~image_type", "compressed") # Setup publisher self.pub_camera_info = rospy.Publisher("/pi_cam/camera_info", CameraInfo, queue_size=1) # Get path to calibration yaml file self.cali_file = rospkg.RosPack().get_path( 'pi_cam' ) + "/camera_info/calibrations/" + self.cali_file_name + ".yaml" self.camera_info_msg = None if not os.path.isfile(self.cali_file): rospy.logwarn( "[%s] Can't find calibration file: %s.\nUsing default calibration instead." % (self.node_name, self.cali_file)) self.cali_file = rospkg.RosPack().get_path( 'pi_cam') + "/camera_info/calibrations/default.yaml" # Shutdown if no calibration file not found if not os.path.isfile(self.cali_file): rospy.signal_shutdown("Found no calibration file ... aborting") # Print out and prepare message rospy.loginfo("[%s] Using calibration file: %s" % (self.node_name, self.cali_file)) # Load calibration yaml file self.camera_info_msg = load_camera_info_2(self.cali_file) self.camera_info_msg.header.frame_id = rospy.get_namespace( ) + "camera_optical_frame" rospy.loginfo("[%s] CameraInfo: %s" % (self.node_name, self.camera_info_msg)) # self.timer_pub = rospy.Timer(rospy.Duration.from_sec(1.0/self.pub_freq),self.cbTimer) img_type = CompressedImage if self.image_type == "compressed" else Image typemsg = "CompressedImage" if self.image_type == "compressed" else "Image" rospy.logwarn("[%s] ==============%s", self.node_name, typemsg) self.sub_img_compressed = rospy.Subscriber("/pi_cam/image_compressed", img_type, self.cbCompressedImage, queue_size=1)
def __init__(self): 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.detector = Detector( searchpath=['/home/pi/workspace/apriltags3-py/apriltags/lib'], families='tag36h11', nthreads=1, quad_decimate=3.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.cameraMatrix = np.array(self.camera_info_msg.K).reshape((3, 3)) self.camera_params = (self.cameraMatrix[0, 0], self.cameraMatrix[1, 1], self.cameraMatrix[0, 2], self.cameraMatrix[1, 2])
def __init__(self): self.camera_info_msg = load_camera_info_2( '../camera_info/calibrations/pi_cam_640x480.yaml') # self.camera_info_msg = load_camera_info_2('../camera_info/calibrations/default.yaml') K = np.array(self.camera_info_msg.K).reshape((3, 3)) # D = np.array(self.camera_info_msg.D[:4]) D = np.array([0., 0., 0., 0.]) P = np.array(self.camera_info_msg.P).reshape((3, 4)) DIM = (self.camera_info_msg.width, self.camera_info_msg.height) K = np.array([[303.646249, 0.000000, 326.026328], [0.000000, 309.237102, 248.168564], [0.000000, 0.000000, 1.000000]]) # self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) img = cv2.imread('/home/ubuntu/test_calibration.png') # print(img) cv2.imshow('raw', img) img_undistorted = cv2.fisheye.undistortImage(img, K, D=D, Knew=K) cv2.imshow('rect', img_undistorted) cv2.waitKey(0) cv2.destroyAllWindows()
def __init__(self): self.node_name = rospy.get_name() rospy.loginfo("[%s] Initializing......" % (self.node_name)) self.framerate_high = self.setupParam("~framerate_high", 30.0) self.framerate_low = self.setupParam("~framerate_low", 15.0) self.res_w = self.setupParam("~res_w", 640) self.res_h = self.setupParam("~res_h", 480) self.image_msg = CompressedImage() self.bridge = CvBridge() # Setup PiCamera self.camera = PiCamera() # self.framerate = self.framerate_low # default to low # self.framerate = self.framerate_high # default to high self.framerate = 3 self.camera.framerate = self.framerate self.camera.resolution = (self.res_w, self.res_h) self.count = 0 self.decode_frame = 7 # For intrinsic calibration self.cali_file_folder = rospkg.RosPack().get_path( 'pi_cam') + "/camera_info/calibrations/" # For load camera info self.cali_file_name = self.setupParam("~cali_file_name", "default") self.cali_file = rospkg.RosPack().get_path( 'pi_cam' ) + "/camera_info/calibrations/" + self.cali_file_name + ".yaml" self.frame_id = rospy.get_namespace().strip( '/') + "/camera_optical_frame" self.has_published = False self.pub_img = rospy.Publisher("~image/compressed", CompressedImage, queue_size=1) self.pub_raw = rospy.Publisher("~image_raw", Image, queue_size=1) self.pub_camera_info = rospy.Publisher("~camera_info", CameraInfo, queue_size=1) # self.sub_switch_high = rospy.Subscriber("~framerate_high_switch", BoolStamped, self.cbSwitchHigh, queue_size=1) self.camera_info_msg = None # Load calibration yaml file rospy.logwarn( "[%s] Can't find calibration file: %s.\nUsing default calibration instead." % (self.node_name, self.cali_file)) # Shutdown if no calibration file not found if not os.path.isfile(self.cali_file): rospy.signal_shutdown("Found no calibration file ... aborting") # Print out and prepare message rospy.loginfo("[%s] Using calibration file: %s" % (self.node_name, self.cali_file)) self.camera_info_msg = load_camera_info_2(self.cali_file) self.camera_info_msg.header.frame_id = self.frame_id # self.camera_info_msg.header.frame_id = rospy.get_namespace() + "camera_optical_frame" rospy.loginfo("[%s] CameraInfo: %s" % (self.node_name, self.camera_info_msg)) # Create service (for camera_calibration) self.srv_set_camera_info = rospy.Service("~set_camera_info", SetCameraInfo, self.cbSrvSetCameraInfo) self.stream = io.BytesIO() self.is_shutdown = False self.update_framerate = False # Setup timer 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.rate = 15 self.bridge = CvBridge() self.frame_id = rospy.get_namespace().strip( '/') + "/camera_optical_frame" self.camera = UsbCamera(callback=self.PublishRaw) self.ic = ImageClassifier() # self.ic.load_model('/root/keras/分类测试/model.h5') self.data_root = self.ic.data_root self.ns = None 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 = None # Image() 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.training_logs_topic = rospy.Publisher("~training_logs", String, queue_size=1) # 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) rospy.Service('~detect_apriltag', GetApriltagDetections, self.cbGetApriltagDetections) rospy.Service('~camera_set_enable', SetInt32, self.srvCameraSetEnable) rospy.Service('~camera_save_frame', SetString, self.srvCameraSaveFrame) rospy.Service('~set_ns', SetString, self.srv_set_ns) rospy.Service('~delete_ns', SetString, self.srv_delete_ns) rospy.Service('~create_cat', SetString, self.srv_create_cat) rospy.Service('~delete_cat', SetString, self.srv_delete_cat) rospy.Service('~list_ns', SetString, self.srv_list_ns) rospy.Service('~list_cat', SetString, self.srv_list_cat) rospy.Service('~train_classifier', SetString, self.srv_train_classifier) rospy.Service('~predict', GetPredictions, self.srv_predict) rospy.Service('~get_training_data', GetPredictions, self.srv_get_training_data) # self.timer_init = rospy.Timer(rospy.Duration.from_sec(1.0/self.tag_detect_rate), self.publish_rect) rospy.loginfo("[%s] Initialized......" % (self.node_name))