def __init__(self, logger, general_config, manager):
self.logger = logger
self.manager = manager
self.general_config = general_config
self.ACQ_TOPIC_WHEEL_COMMAND = self.general_config[
"ACQ_TOPIC_WHEEL_COMMAND"]
self.ACQ_ODOMETRY_TOPIC = self.general_config['ACQ_ODOMETRY_TOPIC']
self.INPUT_BAG_PATH = self.general_config['INPUT_BAG_PATH']
self.OUTPUT_BAG_PATH = self.general_config['OUTPUT_BAG_PATH']
self.ACQ_TOPIC_RAW = self.general_config['ACQ_TOPIC_RAW']
self.ACQ_TOPIC_CAMERAINFO = self.general_config['ACQ_TOPIC_CAMERAINFO']
self.ACQ_TEST_STREAM = self.general_config['ACQ_TEST_STREAM']
self.ACQ_BEAUTIFY = self.general_config['ACQ_BEAUTIFY']
self.ACQ_TAG_SIZE = self.general_config['ACQ_TAG_SIZE']
self.ACQ_POSES_TOPIC = self.general_config['ACQ_POSES_TOPIC']
self.aprilTagProcessor = apriltags3.Detector(
searchpath=[ACQ_APRILTAG_SO],
families='tag36h11',
nthreads=4,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.apriltag_pose_topic = str("/poses_acquisition/" +
self.ACQ_POSES_TOPIC)
self.odometry_topic = str("/poses_acquisition/" +
self.ACQ_ODOMETRY_TOPIC)
self.list_of_agent_apriltag = []
self.list_of_agent_odometry = []
self.sub_config = {}
self.sub_config["ACQ_TOPIC_RAW"] = self.general_config[
'ACQ_TOPIC_RAW'] + "/remapped"
self.sub_config["ACQ_TOPIC_CAMERAINFO"] = self.general_config[
'ACQ_TOPIC_CAMERAINFO'] + "/remapped"
self.sub_config["ACQ_TOPIC_WHEEL_COMMAND"] = self.general_config[
'ACQ_TOPIC_WHEEL_COMMAND'] + "/remapped"
self.sub_config["ACQ_TEST_STREAM"] = self.general_config[
'ACQ_TEST_STREAM']
self.sub_config["ACQ_BEAUTIFY"] = self.general_config['ACQ_BEAUTIFY']
self.sub_config["ACQ_TAG_SIZE"] = self.general_config['ACQ_TAG_SIZE']
self.sub_config["INPUT_BAG_PATH"] = None
self.sub_config["OUTPUT_BAG_PATH"] = self.general_config[
'OUTPUT_BAG_PATH']
self.sub_config["ACQ_POSES_TOPIC"] = self.general_config[
'ACQ_POSES_TOPIC']
self.sub_config["ACQ_ODOMETRY_TOPIC"] = self.general_config[
'ACQ_ODOMETRY_TOPIC']
self.set_list_of_agent()
# self.logger.info(self.list_of_agent_apriltag)
self.node_list = []
pass
def __init__(self, options, logger):
self.logger = logger
self.ImageRectifier = None
self.opt_beautify = options.get('beautify', False)
self.tag_size = options.get('tag_size', 0.065)
self.aprilTagProcessor = apriltags3.Detector(searchpath=[ACQ_APRILTAG_SO],
families='tag36h11',
nthreads=4,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
def fisheye_distortion(intrinsics):
return np.array(intrinsics.coeffs[:4])
#######################################
# Functions for AprilTag detection
#######################################
tag_landing_id = 0
tag_landing_size = 0.144 # tag's border size, measured in meter
tag_image_source = "right" # for Realsense T265, we can use "left" or "right"
at_detector = apriltags3.Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
#######################################
# Functions for MAVLink
#######################################
# Define function to send landing_target mavlink message for mavlink based precision landing
# http://mavlink.org/messages/common#LANDING_TARGET
def send_land_target_message():
global current_time, H_camera_tag, is_landing_tag_detected
if is_landing_tag_detected == True:
def __init__(self, logger, config):
self.config = config
self.ACQ_DEVICE_NAME = self.config['ACQ_DEVICE_NAME']
self.ACQ_TOPIC_RAW = self.config['ACQ_TOPIC_RAW']
self.ACQ_TOPIC_CAMERAINFO = self.config['ACQ_TOPIC_CAMERAINFO']
self.ACQ_TEST_STREAM = self.config['ACQ_TEST_STREAM']
self.ACQ_BEAUTIFY = self.config['ACQ_BEAUTIFY']
self.ACQ_TAG_SIZE = self.config['ACQ_TAG_SIZE']
self.ACQ_POSES_TOPIC = self.config['ACQ_POSES_TOPIC']
self.ACQ_APRILTAG_QUAD_DECIMATE = self.config[
'ACQ_APRILTAG_QUAD_DECIMATE']
self.ACQ_POSES_UPDATE_RATE = self.config['ACQ_POSES_UPDATE_RATE']
self.INPUT_BAG_PATH = config['INPUT_BAG_PATH']
self.OUTPUT_BAG_PATH = config['OUTPUT_BAG_PATH']
self.aprilTagProcessor = apriltags3.Detector(
searchpath=[ACQ_APRILTAG_SO],
families='tag36h11',
nthreads=4,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Initialize ROS nodes and subscribe to topics
rospy.init_node('apriltag_processor_node',
anonymous=True,
disable_signals=True)
self.publish_queue = multiprocessing.Queue()
self.image_queue = multiprocessing.Queue()
self.publishers = {}
self.apriltag_pose_topic = str("/poses_acquisition/" +
self.ACQ_POSES_TOPIC)
self.publishers["apriltags"] = rospy.Publisher(
self.apriltag_pose_topic, AprilTagExtended, queue_size=20)
# If the test stream is requested
if self.ACQ_TEST_STREAM:
self.publishers["test_stream_image"] = rospy.Publisher(
"/poses_acquisition/test_video/" + self.ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
self.publishers["raw_image"] = rospy.Publisher(
"/poses_acquisition/raw_video/" + self.ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
self.publishers["rectified_image"] = rospy.Publisher(
"/poses_acquisition/rectified_video/" + self.ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
self.publishers["raw_camera_info"] = rospy.Publisher(
"/poses_acquisition/camera_info_raw/" + self.ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
self.publishers["new_camera_matrix"] = rospy.Publisher(
"/poses_acquisition/camera_info_rectified/" +
self.ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
# Initialize attributes
self.lastCameraInfo = None
self.logger = logger
self.logger.info('/' + self.ACQ_DEVICE_NAME + '/' + self.ACQ_TOPIC_RAW)
self.seq_stamper = 0
self.image_processor_list = []
# Initialize the device side processor
self.imageprocessor_options = {
'beautify': self.ACQ_BEAUTIFY,
'tag_size': self.ACQ_TAG_SIZE
}
rospy.on_shutdown(self.on_shutdown)
for i in range(8):
new_image_processor = ImageProcessor(self.imageprocessor_options,
self.logger, self.publishers,
self.aprilTagProcessor,
self.publish_queue,
self.config, self.image_queue)
self.image_processor_list.append(new_image_processor)
new_image_processor.start()
self.camera_topic = str('/' + self.ACQ_DEVICE_NAME + '/' +
self.ACQ_TOPIC_RAW)
self.camera_info_topic = str('/' + self.ACQ_DEVICE_NAME + '/' +
self.ACQ_TOPIC_CAMERAINFO)
self.subscriberImage = rospy.Subscriber(self.camera_topic,
CompressedImage,
self.camera_image_callback,
queue_size=50)
self.subscriberCameraInfo = rospy.Subscriber(self.camera_info_topic,
CameraInfo,
self.camera_info_callback,
queue_size=50)
if self.INPUT_BAG_PATH is not None:
self.bag_reader = Process(target=self.read_bag)
self.bag_reader.start()
if self.OUTPUT_BAG_PATH is not None:
self.outputbag = rosbag.Bag(self.OUTPUT_BAG_PATH, 'w')
self.last_call_back_time = rospy.get_time()
self.logger.info('Apriltag processor node is set up.')
self.publish_loop()
def detector():
return apriltags3.Detector()
import numpy as np
from cv2 import imshow
from model.card import Card
from log import log
visualization = False
current_loc = os.path.dirname(os.path.abspath(__file__))
log(current_loc)
file_name = 'turn0.jpg'
read_path = current_loc + "/vision/data/board_rec/before/" + file_name
save_path = current_loc + "/vision/data/board_rec/after/" + file_name
at_detector = apriltags3.Detector(families='tagStandard41h12',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=1)
time_num = 0
time_sum = 0
img = cv2.imread(read_path, cv2.IMREAD_GRAYSCALE)
def id_to_card(id_):
assert id_ < 60
assert id_ >= 0
colors = ['green', 'blue', 'red', 'yellow', 'white']
def __init__(self, logger):
# Get the environment variables
self.ACQ_DEVICE_NAME = os.getenv('ACQ_DEVICE_NAME', 'watchtower10')
self.ACQ_TOPIC_RAW = os.getenv('ACQ_TOPIC_RAW',
'camera_node/image/compressed')
self.ACQ_TOPIC_CAMERAINFO = os.getenv('ACQ_TOPIC_CAMERAINFO',
'camera_node/camera_info')
self.ACQ_TEST_STREAM = bool(int(os.getenv('ACQ_TEST_STREAM', 1)))
self.ACQ_BEAUTIFY = bool(int(os.getenv('ACQ_BEAUTIFY', 1)))
self.ACQ_TAG_SIZE = float(os.getenv('ACQ_TAG_SIZE', 0.065))
self.ACQ_POSES_TOPIC = os.getenv('ACQ_POSES_TOPIC', "poses")
self.ACQ_APRILTAG_QUAD_DECIMATE = float(
os.getenv('ACQ_APRILTAG_QUAD_DECIMATE', 1.0))
self.aprilTagProcessor = apriltags3.Detector(
searchpath=[ACQ_APRILTAG_SO],
families='tag36h11',
nthreads=4,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.ACQ_POSES_UPDATE_RATE = float(
os.getenv('ACQ_POSES_UPDATE_RATE', 10)) # Hz
# Initialize ROS nodes and subscribe to topics
rospy.init_node('apriltag_processor_node',
anonymous=True,
disable_signals=True)
self.subscriberRawImage = rospy.Subscriber('/' + self.ACQ_DEVICE_NAME +
'/' + self.ACQ_TOPIC_RAW,
CompressedImage,
self.camera_image_callback,
queue_size=50)
self.subscriberCameraInfo = rospy.Subscriber(
'/' + self.ACQ_DEVICE_NAME + '/' + self.ACQ_TOPIC_CAMERAINFO,
CameraInfo,
self.camera_info_callback,
queue_size=50)
self.publish_queue = multiprocessing.Queue()
self.publishers = {}
self.publishers["apriltags"] = rospy.Publisher("/poses_acquisition/" +
self.ACQ_POSES_TOPIC,
AprilTagDetection,
queue_size=20)
# If the test stream is requested
if self.ACQ_TEST_STREAM:
self.publishers["test_stream_image"] = rospy.Publisher(
"/poses_acquisition/test_video/" + self.ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
self.publishers["raw_image"] = rospy.Publisher(
"/poses_acquisition/raw_video/" + self.ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
self.publishers["rectified_image"] = rospy.Publisher(
"/poses_acquisition/rectified_video/" + self.ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
self.publishers["raw_camera_info"] = rospy.Publisher(
"/poses_acquisition/camera_info_raw/" + self.ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
self.publishers["new_camera_matrix"] = rospy.Publisher(
"/poses_acquisition/camera_info_rectified/" +
self.ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
# Initialize attributes
self.lastCameraInfo = None
self.logger = logger
self.logger.info('/' + self.ACQ_DEVICE_NAME + '/' + self.ACQ_TOPIC_RAW)
self.seq_stamper = 0
self.image_processor_list = []
# Initialize the device side processor
self.imageprocessor_options = {
'beautify': self.ACQ_BEAUTIFY,
'tag_size': self.ACQ_TAG_SIZE
}
rospy.on_shutdown(self.on_shutdown)
self.logger.info('Apriltag processor node is set up.')
self.publish_loop()
