def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
self.at_detector = Detector(families='tag36h11',
nthreads=5,
quad_decimate=2.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.bridge = CvBridge()
self.pub_tagimg = rospy.Publisher('at_detect/image/compressed',
CompressedImage,
queue_size=1)
self.sub_img = rospy.Subscriber('camera_node/image/compressed',
CompressedImage, self.cb_at_detect)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
self.bridge = CvBridge()
self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0,
quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0)
# subscribe to camera stream
self.sub_camera_img = rospy.Subscriber("camera_node/image/compressed", CompressedImage, self.callback,
queue_size=1)
# publish modified image
self.pub_modified_img = rospy.Publisher(f"~image/compressed", CompressedImage,
queue_size=1)
calibration_data = self.read_yaml_file(f"/data/config/calibrations/camera_intrinsic/{self.veh}.yaml")
self.K = np.array(calibration_data["camera_matrix"]["data"]).reshape(3, 3)
self.log("Letsgoooooo")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(AprilTag_Localization, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
# read calibration
intrinsic_fname = '/data/config/calibrations/camera_intrinsic/default.yaml'
if 'INTRINSICS_FILE' in os.environ:
intrinsic_fname = os.environ['CALIBRATION_FILE']
intrinsics = self.readYamlFile(intrinsic_fname)
self.D = np.array(intrinsics['distortion_coefficients']['data'])
self.K = np.reshape(np.array(intrinsics['camera_matrix']['data']),
[3, 3])
# init poses
self.x = 0
self.y = 0
self.theta = 0
# transforms
self.A_to_map = tf.transformations.identity_matrix() # april-tag
self.A_to_map[2, 3] = -0.095
self.Ad_to_A = np.array([[0, 1, 0, 0], [0, 0, -1, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.C_to_Cd = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 0, 1]])
self.baselink_to_camera = tf.transformations.rotation_matrix(
np.pi * (20 / 180), (0, 1, 0))
self.baselink_to_camera[0:3, 3] = np.array([0.0582, 0, 0.1072])
# april tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# subscriber for images
self.listener = tf.TransformListener()
self.sub = rospy.Subscriber(
f'/{self.veh_name}/camera_node/image/compressed', CompressedImage,
self.image_callback)
# publisher
self.pub_pose = rospy.Publisher(f'/{self.veh_name}/pose/apriltag',
TransformStamped,
queue_size=30)
self.br = tf.TransformBroadcaster()
# other important things
self.cvbr = CvBridge()
self.log("Initialized.")
def __init__(self):
cur_dir = os.path.dirname(os.path.abspath(__file__))
# self.cali_file = rospkg.RosPack().get_path('pi_cam') + "/camera_info/calibrations/default.yaml"
# self.cali_file = "default.yaml"
self.camera_info_msg = load_camera_info_3()
# self.detector = Detector(
# print(cur_dir + '/../../../../devel_isolated/apriltag/lib')
# self.detector = Detector(searchpath=[cur_dir + '/../../../../devel_isolated/apriltag/lib'],
self.detector = Detector(
# self.detector = Detector(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])
self.image = None
self.detections = []
self.tags = []
self.tag = None
if hasattr(R, 'from_dcm'):
self.from_dcm_or_matrix = R.from_dcm
else:
self.from_dcm_or_matrix = R.from_matrix
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# Load calibration files
self._res_w = 640
self._res_h = 480
self.calibration_call()
# Initialize classes
self.at_detec = Detector()
self.helper = Helpers(self.current_camera_info)
self.bridge = CvBridge()
# Subscribe to the compressed camera image
self.cam_image = rospy.Subscriber(
f'/{self.veh}/camera_node/image/compressed',
CompressedImage,
self.callback,
queue_size=10)
# Publishers
self.aug_image = rospy.Publisher(
f'/{self.veh}/{node_name}/image/compressed',
CompressedImage,
queue_size=10)
def StreamProcessedImages(self, request, context):
try:
detector = Detector(searchpath=['/usr/local/lib'],
families='tag36h11',
nthreads=3,
quad_decimate=1.0,
quad_sigma=0.8,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
with picamera.PiCamera(resolution=(width, height),
framerate=1) as camera:
# camera.resolution = (width, height)
print("camera", camera)
# Start a preview and let the camera warm up for 2 seconds
camera.start_preview()
time.sleep(2)
with picamera.array.PiRGBArray(camera) as stream:
for foo in camera.capture_continuous(stream,
'rgb',
use_video_port=True):
stream.flush()
# gray = np.mean(stream.array, axis=2, dtype=np.uint8)
gray = rgb2gray(stream.array)
K = (329.8729619143081, 332.94611303946357, 528.0,
396.0)
detections = detector.detect(gray,
estimate_tag_pose=True,
camera_params=K,
tag_size=0.08)
print("gray.shape", gray.shape)
print("stream.array.shape", stream.array.shape)
gray3 = np.zeros((height, width, 3))
gray3[:, :, 1] = gray
print("detected {} images".format(len(detections)))
image_data = stream.array
# image_data = image_data.reshape((320, 2,
# 240, 2, 3)).max(3).max(1)
proto_image = things_pb2.Image(
width=width,
height=height,
image_data=image_data.tobytes())
proto_detections = map(detection_to_proto, detections)
message = things_pb2.ProcessedImage(
image=proto_image,
apriltag_detections=proto_detections)
stream.seek(0)
yield message
except KeyboardInterrupt:
print('interrupted!')
except Exception as e:
print(e)
raise e
finally:
print('ended')
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
rospy.loginfo("[ARNode]: Initializing Renderer ...")
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# April Tag Detector
rospy.loginfo("[ARNode]: Initializing Detector ...")
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# CV Bridge
rospy.loginfo("[ARNode]: Initializing CV Bridge ...")
self.bridge = CvBridge()
# Intrinsics
rospy.loginfo("[ARNode]: Loading Camera Intrinsics ...")
if (not os.path.isfile(
f'/data/config/calibrations/camera_intrinsic/{self.veh}.yaml')
):
rospy.logwarn(
f'[AugmentedRealityBasics]: Could not find {self.veh}.yaml. Loading default.yaml'
)
camera_intrinsic = self.readYamlFile(
f'/data/config/calibrations/camera_intrinsic/default.yaml')
else:
camera_intrinsic = self.readYamlFile(
f'/data/config/calibrations/camera_intrinsic/{self.veh}.yaml')
self.K = np.array(camera_intrinsic['camera_matrix']['data']).reshape(
3, 3)
# Subscribers
rospy.loginfo("[ARNode]: Initializing Subscribers ...")
self.image_subscriber = rospy.Subscriber(
'camera_node/image/compressed', CompressedImage, self.callback)
# Publishers
rospy.loginfo("[ARNode]: Initializing Publishers ...")
self.mod_img_pub = rospy.Publisher('ar_node/image/compressed',
CompressedImage,
queue_size=1)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.camera_sub = rospy.Subscriber('camera_node/image/compressed',
CompressedImage, self.callback)
# construct publisher for AR images
self.pub = rospy.Publisher('~augemented_image/compressed',
CompressedImage,
queue_size=10)
# april-tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=2.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# get camera calibration parameters (homography, camera matrix, distortion parameters)
self.intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
rospy.loginfo('Reading camera intrinsics from {}'.format(
self.intrinsics_file))
intrinsics = self.readYamlFile(self.intrinsics_file)
self.h = intrinsics['image_height']
self.w = intrinsics['image_width']
self.camera_mat = np.array(
intrinsics['camera_matrix']['data']).reshape(3, 3)
# Precompute some matricies
self.camera_params = (self.camera_mat[0, 0], self.camera_mat[1, 1],
self.camera_mat[0, 2], self.camera_mat[1, 2])
self.inv_camera_mat = np.linalg.inv(self.camera_mat)
self.mat_3Dto2D = np.concatenate((np.identity(3), np.zeros((3, 1))),
axis=1)
self.T = np.zeros((4, 4))
self.T[-1, -1] = 1.0
def __init__(self):
self.coral_model = {}
self.coral_labels = {}
self.caffe_model = {}
self.at_detector = {}
self.videoStream = {}
self.status = None
self.captureFrame = None
self.visionFrame = None
self.thread = Thread(target=self.frameUpdate,args=())
self.thread.daemon = True
self.flaskThread = Thread(target=self.runFlask)
self.flaskThread.daemon = True
self.frameLock = Lock()
print("[INFO] Initialising ROS...")
#self.pub = rospy.Publisher(name='vision_detect',subscriber_listener=vision_detect,queue_size=5,data_class=vision_detect)
self.pub = rospy.Publisher('/vision_detect',vision_detect)
rospy.init_node('robot5_vision',anonymous=False)
for row in open(self.coral_labels_file):
(classID, label) = row.strip().split(maxsplit=1)
self.coral_labels[int(classID)] = label.strip()
print("[INFO] loading Coral model...")
self.coral_model = DetectionEngine(self.coral_model_file)
#print("[INFO] loading Caffe model...")
#self.caffe_model = cv2.dnn.readNetFromCaffe(self.caffe_prototxt, self.caffe_model_file)
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
print("[INFO] Running Flask...")
self.app = Flask(__name__)
self.add_routes()
self.flaskThread.start()
print("[INFO] starting video stream...")
self.videoStream = VideoStream(src=0,usePiCamera=True).start()
time.sleep(2.0) # warmup
self.captureFrame = self.videoStream.read()
self.visionFrame = self.captureFrame
self.thread.start()
time.sleep(0.5) # get first few
srun = True
print("[INFO] running frames...")
while srun:
srun = self.doFrame()
cv2.destroyAllWindows()
self.videoStream.stop()
def __init__(self):
super(AprilTagDetector,
self).__init__(node_name='apriltag_detector_node',
node_type=NodeType.PERCEPTION)
# get static parameters
self.family = rospy.get_param('~family', 'tag36h11')
self.nthreads = rospy.get_param('~nthreads', 1)
self.quad_decimate = rospy.get_param('~quad_decimate', 1.0)
self.quad_sigma = rospy.get_param('~quad_sigma', 0.0)
self.refine_edges = rospy.get_param('~refine_edges', 1)
self.decode_sharpening = rospy.get_param('~decode_sharpening', 0.25)
self.tag_size = rospy.get_param('~tag_size', 0.065)
# dynamic parameter
self.detection_freq = DTParam('~detection_freq',
default=-1,
param_type=ParamType.INT,
min_value=-1,
max_value=30)
self._detection_reminder = DTReminder(
frequency=self.detection_freq.value)
# camera info
self._camera_parameters = None
self._camera_frame = None
# create detector object
self._at_detector = Detector(families=self.family,
nthreads=self.nthreads,
quad_decimate=self.quad_decimate,
quad_sigma=self.quad_sigma,
refine_edges=self.refine_edges,
decode_sharpening=self.decode_sharpening)
# create a CV bridge object
self._bridge = CvBridge()
# create subscribers
self._img_sub = rospy.Subscriber('image_rect', Image, self._img_cb)
self._cinfo_sub = rospy.Subscriber('camera_info', CameraInfo,
self._cinfo_cb)
# create publishers
self._img_pub = rospy.Publisher('tag_detections_image/compressed',
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION)
self._tag_pub = rospy.Publisher('tag_detections',
AprilTagDetectionArray,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self._img_pub_busy = False
# create TF broadcaster
self._tf_bcaster = tf.TransformBroadcaster()
# spin forever
rospy.spin()
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATPoseNode, self).__init__(node_name=node_name,
node_type=NodeType.LOCALIZATION)
# get the name of the robot
self.veh = rospy.get_namespace().strip("/")
self.bridge = CvBridge()
self.camera_info = None
self.Rectify = None
self.at_detector = Detector(families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Construct publishers
self.at_estimated_pose = rospy.Publisher(
f'/{self.veh}/at_localization',
Odometry,
queue_size=1,
dt_topic_type=TopicType.LOCALIZATION)
# Camera subscribers:
camera_topic = f'/{self.veh}/camera_node/image/compressed'
self.camera_feed_sub = rospy.Subscriber(camera_topic,
CompressedImage,
self.detectATPose,
queue_size=1,
buff_size=2**24)
camera_info_topic = f'/{self.veh}/camera_node/camera_info'
self.camera_info_sub = rospy.Subscriber(camera_info_topic,
CameraInfo,
self.getCameraInfo,
queue_size=1)
self.image_pub = rospy.Publisher(f'/{self.veh}/rectified_image',
Image,
queue_size=10)
self.tag_pub = rospy.Publisher(f'/{self.veh}/detected_tags',
Int32MultiArray,
queue_size=5)
self.log("Initialized!")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
self.bridge = CvBridge()
#
# Write your code here
#
self.homography = self.load_extrinsics()
self.H = np.array(self.homography).reshape((3, 3))
self.Hinv = np.linalg.inv(self.H)
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.sub_compressed_img = rospy.Subscriber(
"/robot_name/camera_node/image/compressed",
CompressedImage,
self.callback,
queue_size=1
)
self.sub_camera_info = rospy.Subscriber(
"/robot_name/camera_node/camera_info",
CameraInfo,
self.cb_camera_info,
queue_size=1
)
self.pub_map_img = rospy.Publisher(
"~april_tag_duckie/image/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION,
)
def __init__(self, camera_info, tag_size):
# init image rectification
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(camera_info)
self.K_rect, self.mapx, self.mapy = self._init_rectification()
# init apriltag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.tag_size = tag_size
self.camera_params = (self.K_rect[0, 0], self.K_rect[1, 1],
self.K_rect[0, 2], self.K_rect[1, 2])
def __init__(self, node_name):
"""Wheel Encoder Localization Node
Calculates the pose of the Duckiebot using only AprilTags
"""
# Initialize the DTROS parent class
super(LocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)
self.veh_name = rospy.get_namespace().strip("/")
# define initial stuff
self.camera_x = 0.065
self.camera_z = 0.11
self.camera_slant = 19/180*math.pi
self.height_streetsigns = 0.07
# Load calibration files
self._res_w = 640
self._res_h = 480
self.calibration_call()
# define tf stuff
self.br = tf.TransformBroadcaster()
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
# Initialize classes
self.at_detec = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=4.0,
quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0)
self.helper = Helpers(self.current_camera_info)
self.bridge = CvBridge()
# Subscribers
self.cam_image = rospy.Subscriber(f'/{self.veh_name}/camera_node/image/compressed', CompressedImage,
self.callback, queue_size=10)
# Publishers
self.pub_baselink_pose = rospy.Publisher('~at_baselink_pose', TransformStamped, queue_size=1)
# static broadcasters
self.broadcast_static_transform_baselink()
self.log("Initialized")
def __init__(self, proxy_map, startup_check=False):
super(SpecificWorker, self).__init__(proxy_map)
# Drone
self.state = 'idle'
self.x = 0
self.y = 0
self.pose = {}
self.appleCatched = False
self.treeCatched = False
self.depthX = 180
self.depthY = 180
#AprilTags
self.center = 0
self.tags = {}
self.n_tags = 0
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Image
self.image = []
self.depth = []
self.camera_name = "frontCamera"
self.data = {}
self.depth_array = []
self.color = 0
#Timer
self.times = 0
self.Period = 100
if startup_check:
self.startup_check()
else:
self.timer.timeout.connect(self.compute)
self.timer.start(self.Period)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
#
# Write your code here
#
self.bridge = CvBridge()
self.image = None
self.image_height = None
self.image_width = None
self.cam_info = None
self.camera_info_received = False
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1
)
self.sub_cam_info = rospy.Subscriber(f'/{self.veh}/camera_node/camera_info', CameraInfo,
self.cb_camera_info, queue_size=1)
self.pub_aug_image = rospy.Publisher(f'/{self.veh}/{node_name}/image/compressed',
CompressedImage, queue_size=10)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
# Load calibration files
self.calib_data = self.readYamlFile(
'/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml')
self.log('Loaded intrinsics calibration file')
self.extrinsics = self.readYamlFile(
'/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml')
self.log('Loaded extrinsics calibration file')
# Retrieve intrinsic info
self.cam_info = self.setCamInfo(self.calib_data)
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
self.log('Renderer object created')
# Create AprilTag detector object
self.at_detector = Detector()
# Create cv_bridge
self.bridge = CvBridge()
# Define subscriber to recieve images
self.image_sub = rospy.Subscriber(
'/' + self.veh + '/camera_node/image/compressed', CompressedImage,
self.callback)
# Publish the rendered image to a new topic
self.augmented_pub = rospy.Publisher('~image/compressed',
CompressedImage,
queue_size=1)
self.log(node_name + ' initialized and running')
window.wm_title("Tracking")
window.config()
# Graphics window
imageFrame = tk.Frame(window, width=600, height=500)
imageFrame.grid(row=0, column=0, padx=10, pady=10, columnspan=2)
# Capture video frames
lmain = tk.Label(imageFrame)
lmain.grid(row=0, column=0)
# Set up detector
at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
detections = {}
transforms = {}
translations = {}
c270_params = [1402, 1402, 642, 470] # Probably at odd resolution
brio_params = [499.0239, 499.1960, 310.1258, 232.4641] # At 640x480
tag_size_overrides = {0: 0.092}
master_tags = [14, 36]
def __init__(self, node_name):
super(FusedLocalizationNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.radius = rospy.get_param(
f'/{self.veh_name}/kinematics_node/radius', 100)
self.baseline = 0.0968
x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100)
self.rectify_flag = rospy.get_param(
f'/{self.veh_name}/{node_name}/rectify', 100)
self.encoder_conf_flag = False
self.bridge = CvBridge()
self.image = None
self.image_timestamp = rospy.Time.now()
self.cam_info = None
self.camera_info_received = False
self.newCameraMatrix = None
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
trans_base_cam = np.array([0.0582, 0.0, 0.1072])
rot_base_cam = rotation_from_axis_angle(np.array([0, 1, 0]),
np.radians(15))
rot_cam_base = rot_base_cam.T
trans_cam_base = -rot_cam_base @ trans_base_cam
self.pose_cam_base = SE3_from_rotation_translation(
rot_cam_base, trans_cam_base)
self.tfs_msg_cam_base = TransformStamped()
self.tfs_msg_cam_base.header.frame_id = 'camera'
self.tfs_msg_cam_base.header.stamp = rospy.Time.now()
self.tfs_msg_cam_base.child_frame_id = 'at_baselink'
self.tfs_msg_cam_base.transform = self.pose2transform(
self.pose_cam_base)
self.static_tf_br_cam_base = tf2_ros.StaticTransformBroadcaster()
translation_map_at = np.array([0.0, 0.0, 0.08])
rot_map_at = np.eye(3)
self.pose_map_at = SE3_from_rotation_translation(
rot_map_at, translation_map_at)
self.tfs_msg_map_april = TransformStamped()
self.tfs_msg_map_april.header.frame_id = 'map'
self.tfs_msg_map_april.header.stamp = rospy.Time.now()
self.tfs_msg_map_april.child_frame_id = 'apriltag'
self.tfs_msg_map_april.transform = self.pose2transform(
self.pose_map_at)
self.static_tf_br_map_april = tf2_ros.StaticTransformBroadcaster()
self.tfs_msg_april_cam = TransformStamped()
self.tfs_msg_april_cam.header.frame_id = 'apriltag'
self.tfs_msg_april_cam.header.stamp = rospy.Time.now()
self.tfs_msg_april_cam.child_frame_id = 'camera'
self.br_april_cam = tf.TransformBroadcaster()
self.tfs_msg_map_base = TransformStamped()
self.tfs_msg_map_base.header.frame_id = 'map'
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.tfs_msg_map_base.child_frame_id = 'fused_baselink'
self.br_map_base = tf.TransformBroadcaster()
self.pose_map_base_SE2 = SE2_from_xytheta([x_init, 0, np.pi])
R_x_c = rotation_from_axis_angle(np.array([1, 0, 0]), np.pi / 2)
R_z_c = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_y_a = rotation_from_axis_angle(np.array([0, 1, 0]), -np.pi / 2)
R_z_a = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_dtc_c = R_x_c @ R_z_c
R_a_dta = R_y_a @ R_z_a
self.T_dtc_c = SE3_from_rotation_translation(R_dtc_c,
np.array([0, 0, 0]))
self.T_a_dta = SE3_from_rotation_translation(R_a_dta,
np.array([0, 0, 0]))
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh_name}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_cam_info = rospy.Subscriber(
f'/{self.veh_name}/camera_node/camera_info',
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.pub_tf_fused_loc = rospy.Publisher(
f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped,
queue_size=10)
else:
try:
print(parameters[args.camera]['cam_name'])
mod = import_module("lib." + parameters[args.camera]['cam_name'])
camera = mod.camera(parameters[args.camera])
except (ImportError, KeyError):
print('No camera with the name {0}, exiting'.format(args.camera))
sys.exit(0)
# allow the camera to warmup
time.sleep(2)
at_detector = Detector(searchpath=['apriltags3py/apriltags/lib', 'apriltags3py/apriltags/lib'],
families='tagStandard41h12',
nthreads=3,
quad_decimate=args.decimation,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# how many loops
loops = camera.getNumberImages() if camera.getNumberImages() else args.loop
print("Starting {0} image capture and process...".format(loops))
outfile = open(args.outfile,"w+")
outfile.write("{0},{1},{2},{3},{4},{5},{6},{7},{8}\n".format("Filename", "TagID", "PosX (left)", "PosY (up)", "PosZ (fwd)", "RotX (pitch)", "RotY (yaw)", "RotZ (roll)", "PoseErr"))
# Need to reconstruct K and D
if camParams['fisheye']:
K = numpy.zeros((3, 3))
ap.add_argument("-v", "--video", help="path to the (optional) video file")
ap.add_argument("-b", "--buffer", type=int, default=64, help="max buffer size")
args = vars(ap.parse_args())
# define the apriltags detector for use
# at_detector = Detector(searchpath=['apriltags'],
# families='tag36h11',
# nthreads=1,
# quad_decimate=1.0,
# quad_sigma=0.0,
# refine_edges=1,
# decode_sharpening=0.25,
# debug=0)
# at_detector = Detector(families='tag16h5') # initialize detector with our tag family
at_detector = Detector(
families='tagStandard41h12') # initialize detector with our tag family
focal_length = 3.04 # mm
sensor_res = (3280, 2464) # pixels
sensor_area = (3.68, 2.76) # mm
fx = focal_length * sensor_res[0] / sensor_area[0]
fy = focal_length * sensor_res[1] / sensor_area[1]
cx = sensor_res[0] / 2
cy = sensor_res[1] / 2
picam_params = (fx, fy, cx, cy)
tag_size = 6 / 3.2808 # 6in to meters
# if a video path was not supplied, grab the reference
# to the webcam
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.node_name = rospy.get_name().strip("/")
# initialize renderer
rospack = rospkg.RosPack()
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# initialize apriltag detector
self.at_detector = Detector(
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0,
#searchpath=[]
)
self.at_camera_params = None
# self.at_tag_size = 0.065 # fixed param
self.at_tag_size = 2 # to scale pose matrix to homography
# initialize member variables
self.bridge = CvBridge()
self.camera_info_received = False
self.camera_info = None
self.camera_P = None
self.camera_K = None
self.cv2_img = None
# initialize subs and pubs
self.sub_compressed_img = Subscriber(
"/{}/camera_node/image/compressed".format(self.veh_name),
CompressedImage,
self.cb_image,
queue_size=1)
self.loginfo("Subcribing to topic {}".format(
"/{}/camera_node/image/compressed".format(self.veh_name)))
self.sub_camera_info = Subscriber("/{}/camera_node/camera_info".format(
self.veh_name),
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.loginfo("Subcribing to topic {}".format(
"/{}/camera_node/camera_info".format(self.veh_name)))
self.pub_aug_img = Publisher(
"/{}/{}/augmented_image/compressed".format(self.veh_name,
self.node_name),
CompressedImage,
queue_size=1)
self.loginfo("Publishing to topic {}".format(
"/{}/{}/augmented_image/compressed".format(self.veh_name,
self.node_name)))
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATLocalizationNode, self).__init__(
node_name=node_name, node_type=NodeType.PERCEPTION)
self.veh = rospy.get_namespace().strip("/")
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.camera_sub = rospy.Subscriber(
'camera_node/image/compressed', CompressedImage, self.callback, queue_size=1, buff_size=(20*(1024**2)))
# self.debug_pub = rospy.Publisher(
# '~debug_image/compressed', CompressedImage)
# tf broadcasters
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
self.tf_br = tf2_ros.TransformBroadcaster()
# april-tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# keep track of the ID of the landmark tag
self.at_id = None
# get camera calibration parameters (homography, camera matrix, distortion parameters)
intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
extrinsics_file = '/data/config/calibrations/camera_extrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
rospy.loginfo('Reading camera intrinsics from {}'.format(
intrinsics_file))
rospy.loginfo('Reading camera extrinsics from {}'.format(
extrinsics_file))
intrinsics = readYamlFile(intrinsics_file)
extrinsics = readYamlFile(extrinsics_file)
self.h = intrinsics['image_height']
self.w = intrinsics['image_width']
cam_mat = np.array(
intrinsics['camera_matrix']['data']).reshape(3, 3)
distortion_coeff = np.array(
intrinsics['distortion_coefficients']['data'])
H_ground2img = np.linalg.inv(
np.array(extrinsics['homography']).reshape(3, 3))
# precompute some quantities
new_cam_mat, _ = cv2.getOptimalNewCameraMatrix(
cam_mat, distortion_coeff, (640, 480), 1.0)
self.map1, self.map2, = cv2.initUndistortRectifyMap(
cam_mat, distortion_coeff, np.eye(3), new_cam_mat, (640, 480), cv2.CV_32FC1)
self.camera_params = (
new_cam_mat[0, 0], new_cam_mat[1, 1], new_cam_mat[0, 2], new_cam_mat[1, 2])
# define and broadcast static tfs
self.camloc_camcv = np.array([[0.0, 0.0, 1.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
self.atcv_atloc = np.array([[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
camcv_base_estimated = estimateTfFromHomography(
H_ground2img, new_cam_mat)
theta = 15.0 / 180.0 * np.pi
base_camloc_nominal = np.array([[np.cos(theta), 0.0, np.sin(theta), 0.0582],
[0.0, 1.0, 0.0, 0.0],
[-np.sin(theta), 0.0,
np.cos(theta), 0.1072],
[0.0, 0.0, 0.0, 1.0]])
self.camloc_base = self.camloc_camcv @ camcv_base_estimated
# self.camloc_base = np.linalg.inv(base_camloc_nominal)
self.map_atloc = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.085],
[0.0, 0.0, 0.0, 1.0]])
broadcastTF(self.map_atloc, 'map', 'april_tag', self.static_tf_br)
def __init__(self):
super(AprilTagDetector, self).__init__(
node_name='apriltag_detector_node',
node_type=NodeType.PERCEPTION
)
# get static parameters
self.family = rospy.get_param('~family', 'tag36h11')
self.ndetectors = rospy.get_param('~ndetectors', 1)
self.nthreads = rospy.get_param('~nthreads', 1)
self.quad_decimate = rospy.get_param('~quad_decimate', 1.0)
self.quad_sigma = rospy.get_param('~quad_sigma', 0.0)
self.refine_edges = rospy.get_param('~refine_edges', 1)
self.decode_sharpening = rospy.get_param('~decode_sharpening', 0.25)
self.tag_size = rospy.get_param('~tag_size', 0.065)
self.rectify_alpha = rospy.get_param('~rectify_alpha', 0.0)
# dynamic parameter
self.detection_freq = DTParam(
'~detection_freq',
default=-1,
param_type=ParamType.INT,
min_value=-1,
max_value=30
)
self._detection_reminder = DTReminder(frequency=self.detection_freq.value)
# camera info
self._camera_parameters = None
self._mapx, self._mapy = None, None
# create detector object
self._detectors = [Detector(
families=self.family,
nthreads=self.nthreads,
quad_decimate=self.quad_decimate,
quad_sigma=self.quad_sigma,
refine_edges=self.refine_edges,
decode_sharpening=self.decode_sharpening
) for _ in range(self.ndetectors)]
self._renderer_busy = False
# create a CV bridge object
self._jpeg = TurboJPEG()
# create subscribers
self._img_sub = rospy.Subscriber(
'~image',
CompressedImage,
self._img_cb,
queue_size=1,
buff_size='20MB'
)
self._cinfo_sub = rospy.Subscriber(
'~camera_info',
CameraInfo,
self._cinfo_cb,
queue_size=1
)
# create publisher
self._tag_pub = rospy.Publisher(
'~detections',
AprilTagDetectionArray,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION,
dt_help='Tag detections',
)
self._img_pub = rospy.Publisher(
'~detections/image/compressed',
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION,
dt_help='Camera image with tag publishs superimposed',
)
# create thread pool
self._workers = ThreadPoolExecutor(self.ndetectors)
self._tasks = [None] * self.ndetectors
# create TF broadcaster
self._tf_bcaster = tf.TransformBroadcaster()
# import the opencv library
import cv2
from dt_apriltags import Detector
import numpy as np
import time
import operator
at_detector = Detector(families='tag16h5',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
camera_params = (336.7755634193813, 336.02729840829176, 333.3575643300718,
212.77376312080065)
capture_duration = 8
# define a video capture object
vid = cv2.VideoCapture(0)
# Check if camera opened successfully
if (vid.isOpened() == False):
print("Error opening video file")
def checkForTags():
start_time = time.time()
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATLocalizationNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
# State variable for the robot
self.pose = Pose2D(0.27, 0.0, np.pi) # Initial state given arbitrarily
# Static transforms
# Map -> Baselink. To be computed
self.T_map_baselink = TransformStamped()
self.T_map_baselink.header.frame_id = 'map'
self.T_map_baselink.child_frame_id = 'at_baselink'
# Map -> Apriltag. STATIC
self._T_map_apriltag = TransformStamped()
self._T_map_apriltag.header.frame_id = 'map'
self._T_map_apriltag.header.stamp = rospy.Time.now()
self._T_map_apriltag.child_frame_id = 'apriltag'
self._T_map_apriltag.transform.translation.x = 0.0
self._T_map_apriltag.transform.translation.y = 0.0
self._T_map_apriltag.transform.translation.z = 0.09 # Height of 9 cm
q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
self._T_map_apriltag.transform.rotation.x = q[0]
self._T_map_apriltag.transform.rotation.y = q[1]
self._T_map_apriltag.transform.rotation.z = q[2]
self._T_map_apriltag.transform.rotation.w = q[3]
# Apriltag -> Camera. Computed using apriltag detection
self.T_apriltag_camera = TransformStamped()
self.T_apriltag_camera.header.frame_id = 'apriltag'
self.T_apriltag_camera.child_frame_id = 'camera'
# Camera -> Baselink. STATIC
self._T_camera_baselink = TransformStamped()
self._T_camera_baselink.header.frame_id = 'camera'
self._T_camera_baselink.header.stamp = rospy.Time.now()
self._T_camera_baselink.child_frame_id = 'at_baselink'
self._T_camera_baselink.transform.translation.x = -0.0582
self._T_camera_baselink.transform.translation.y = 0.0
self._T_camera_baselink.transform.translation.z = -0.110
q = tf_conversions.transformations.quaternion_from_euler(
0.0, np.deg2rad(-15), 0.0)
self._T_camera_baselink.transform.rotation.x = q[0]
self._T_camera_baselink.transform.rotation.y = q[1]
self._T_camera_baselink.transform.rotation.z = q[2]
self._T_camera_baselink.transform.rotation.w = q[3]
# Transformation Matrices to ease computation
R_MA = tf_conversions.transformations.euler_matrix(
0.0, 0.0, 0.0, 'rxyz')
t_MA = tf_conversions.transformations.translation_matrix(
np.array([0.0, 0.0, 0.09]))
self.T_MA = tf_conversions.transformations.concatenate_matrices(
t_MA, R_MA)
R_CB = tf_conversions.transformations.euler_matrix(
0.0, np.deg2rad(-15.0), 0.0, 'rxyz')
t_CB = tf_conversions.transformations.translation_matrix(
np.array([-0.0582, 0.0, -0.1072]))
self.T_CB = tf_conversions.transformations.concatenate_matrices(
t_CB, R_CB)
# Define rotation matrices to follow axis convention in rviz when using apriltag detection
self.T_ApA = tf_conversions.transformations.euler_matrix(
np.pi / 2.0, 0.0, -np.pi / 2.0, 'rxyz')
self.T_CCp = tf_conversions.transformations.euler_matrix(
-np.pi / 2.0, 0.0, -np.pi / 2.0, 'rzyx')
# Load calibration files
self.calib_data = self.readYamlFile(
'/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml')
self.log('Loaded intrinsics calibration file')
self.extrinsics = self.readYamlFile(
'/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml')
self.log('Loaded extrinsics calibration file')
# Retrieve intrinsic info
cam_info = self.setCamInfo(self.calib_data)
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(cam_info)
# Initiate maps for rectification
self._init_rectify_maps()
# Create AprilTag detector object
self.at_detector = Detector(families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Create cv_bridge
self.bridge = CvBridge()
# Define subscriber to recieve images
self.image_sub = rospy.Subscriber(
'/' + self.veh + '/camera_node/image/compressed', CompressedImage,
self.callback)
# Publishers and broadcasters
self.pub_robot_pose_tf = rospy.Publisher('~at_baselink_transform',
TransformStamped,
queue_size=1)
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
self.tfBroadcaster = tf.TransformBroadcaster(queue_size=1)
self.log(node_name + ' initialized and running')
