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