def mk_pub_msg(self, msg_z, msg_id): tag_detection_array = AprilTagDetectionArray() tag_pose = PoseStamped() tag_pose.pose.position.z = msg_z tag_detection = AprilTagDetection() tag_detection.pose = tag_pose tag_detection.id = msg_id tag_detection_array.detections.append(tag_detection) self.pub_visualize.publish(tag_detection_array)
def _detect(self, detector_id, msg):
# turn image message into grayscale image
with self.profiler("/cb/image/decode"):
img = self._jpeg.decode(msg.data, pixel_format=TJPF_GRAY)
# run input image through the rectification map
with self.profiler("/cb/image/rectify"):
img = cv2.remap(img, self._mapx, self._mapy, cv2.INTER_NEAREST)
# detect tags
with self.profiler("/cb/image/detection"):
tags = self._detectors[detector_id].detect(img, True,
self._camera_parameters,
self.tag_size)
# pack detections into a message
tags_msg = AprilTagDetectionArray()
tags_msg.header.stamp = msg.header.stamp
tags_msg.header.frame_id = msg.header.frame_id
for tag in tags:
# turn rotation matrix into quaternion
q = _matrix_to_quaternion(tag.pose_R)
p = tag.pose_t.T[0]
# create single tag detection object
detection = AprilTagDetection(
transform=Transform(
translation=Vector3(x=p[0], y=p[1], z=p[2]),
rotation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]),
),
tag_id=tag.tag_id,
tag_family=str(tag.tag_family),
hamming=tag.hamming,
decision_margin=tag.decision_margin,
homography=tag.homography.flatten().astype(
np.float32).tolist(),
center=tag.center.tolist(),
corners=tag.corners.flatten().tolist(),
pose_error=tag.pose_err,
)
# add detection to array
tags_msg.detections.append(detection)
# publish tf
self._tf_bcaster.sendTransform(
p.tolist(),
q.tolist(),
msg.header.stamp,
"tag/{:s}".format(str(tag.tag_id)),
msg.header.frame_id,
)
# publish detections
self._tag_pub.publish(tags_msg)
# update healthy frequency metadata
self._tag_pub.set_healthy_freq(self._img_sub.get_frequency())
self._img_pub.set_healthy_freq(self._img_sub.get_frequency())
# render visualization (if needed)
if self._img_pub.anybody_listening() and not self._renderer_busy:
self._renderer_busy = True
Thread(target=self._render_detections,
args=(msg, img, tags)).start()
def _img_cb(self, data):
if self._camera_parameters is None:
return
if not self._detection_reminder.is_time(
frequency=self.detection_freq.value):
return
# ---
# turn image message into grayscale image
img = self._bridge.imgmsg_to_cv2(data, desired_encoding='mono8')
# detect tags
tags = self._at_detector.detect(img, True, self._camera_parameters,
self.tag_size)
# draw the detections on an image (if needed)
if self._img_pub.anybody_listening() and not self._img_pub_busy:
self._img_pub_busy = True
Thread(target=self._publish_detections_image,
args=(img, tags)).start()
# pack detections into a message
msg = AprilTagDetectionArray()
detection_time = rospy.Time.now()
msg.header.stamp = detection_time
msg.header.frame_id = self._camera_frame
for tag in tags:
# turn rotation matrix into quaternion
q = _matrix_to_quaternion(tag.pose_R)
p = tag.pose_t.T[0]
# create single tag detection object
detection = AprilTagDetection(
transform=Transform(translation=Vector3(x=p[0], y=p[1],
z=p[2]),
rotation=Quaternion(x=q[0],
y=q[1],
z=q[2],
w=q[3])),
tag_id=tag.tag_id,
tag_family=str(tag.tag_family),
hamming=tag.hamming,
decision_margin=tag.decision_margin,
homography=tag.homography.flatten().astype(
np.float32).tolist(),
center=tag.center.tolist(),
corners=tag.corners.flatten().tolist(),
pose_error=tag.pose_err)
# add detection to array
msg.detections.append(detection)
# publish tf
self._tf_bcaster.sendTransform(p.tolist(), q.tolist(),
detection_time,
'/tag{:s}'.format(str(tag.tag_id)),
self._camera_frame)
# publish detections
self._tag_pub.publish(msg)
# update healthy frequency metadata
self._tag_pub.set_healthy_freq(self._img_sub.get_frequency())
self._img_pub.set_healthy_freq(self._img_sub.get_frequency())
def callback(self, msg):
tag_infos = []
new_tag_data = AprilTagsWithInfos()
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
#Can use id 1 as long as no bundles are used
new_info.id = int(detection.id[0])
id_info = self.tags_dict[new_info.id]
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[
id_info['traffic_sign_type']]
# publish for FSM
# parking apriltag event
msg_parking = BoolStamped()
msg_parking.header.stamp = rospy.Time(0)
if new_info.traffic_sign_type == TagInfo.PARKING:
msg_parking.data = True
else:
msg_parking.data = False
self.pub_postPros_parking.publish(msg_parking)
# intersection apriltag event
msg_intersection = BoolStamped()
msg_intersection.header.stamp = rospy.Time(0)
if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (
new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT
) or (new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT
) or (new_info.traffic_sign_type
== TagInfo.T_INTERSECTION):
msg_intersection.data = True
else:
msg_intersection.data = False
self.pub_postPros_intersection.publish(msg_intersection)
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.pose.position
rot = detection.pose.pose.pose.orientation
header = detection.pose.header
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
new_tag_data.detections.append(
AprilTagDetection(int(detection.id[0]),
float(detection.size[0]),
PoseStamped(header, Pose(trans, rot))))
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
def publishOnServer(outputDictQueue, quitEvent, logger, mode='live'):
"""
Publishes the processed data on the ROS Master that the graph optimizer uses.
"""
assert (mode == 'live' or mode == 'postprocessing')
logger.info("Setting up the server side process")
# Get the environment variables
ACQ_POSES_TOPIC = os.getenv('ACQ_POSES_TOPIC', "poses")
ACQ_ODOMETRY_TOPIC = os.getenv('ACQ_ODOMETRY_TOPIC', "odometry")
ACQ_DEVICE_NAME = os.getenv('ACQ_DEVICE_NAME', "watchtower10")
ACQ_TEST_STREAM = bool(int(os.getenv('ACQ_TEST_STREAM', 1)))
ACQ_OBSERVATIONS_STATISTICS_OUTPUT = os.getenv(
'ACQ_OBSERVATIONS_STATISTICS_OUTPUT', None)
seq_stamper = 0
counts = collections.Counter()
# Different initialization of the topics for live and postprocessing modes
if mode == 'live':
# Setup the topics
publisherPoses = rospy.Publisher("/poses_acquisition/" +
ACQ_POSES_TOPIC,
AprilTagDetection,
queue_size=1)
publisherOdometry = rospy.Publisher("/poses_acquisition/" +
ACQ_ODOMETRY_TOPIC,
TransformStamped,
queue_size=1)
# If the test stream is requested
if ACQ_TEST_STREAM:
publisherTestImages = rospy.Publisher(
"/poses_acquisition/test_video/" + ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
publisherRawImages = rospy.Publisher(
"/poses_acquisition/raw_video/" + ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
publisherRectifiedImages = rospy.Publisher(
"/poses_acquisition/rectified_video/" + ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
publisherCameraInfoRaw = rospy.Publisher(
"/poses_acquisition/camera_info_raw/" + ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
publisherCameraInfoRectified = rospy.Publisher(
"/poses_acquisition/camera_info_rectified/" + ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
# Init the node (live mode only)
rospy.init_node('acquisition_node_' + ACQ_DEVICE_NAME)
elif mode == 'postprocessing':
# Open the bag (postprocessing mode only)
ACQ_OUTPUT_BAG = os.getenv('ACQ_OUTPUT_BAG', None)
if ACQ_OUTPUT_BAG == None:
raise Exception(
"ACQ_OUTPUT_BAG must be set if the server processor is in postprocessing mode!"
)
if os.path.exists(ACQ_OUTPUT_BAG):
bag = rosbag.Bag(ACQ_OUTPUT_BAG, 'a')
else:
bag = rosbag.Bag(ACQ_OUTPUT_BAG, 'w')
# Setup the topics
publisherPoses = MockupPublisher(
bag, "/poses_acquisition/" + ACQ_POSES_TOPIC)
publisherOdometry = MockupPublisher(
bag, "/poses_acquisition/" + ACQ_ODOMETRY_TOPIC)
# If the test stream is requested
if ACQ_TEST_STREAM:
publisherTestImages = MockupPublisher(
bag, "/poses_acquisition/test_video/" + ACQ_DEVICE_NAME +
"/compressed")
publisherRawImages = MockupPublisher(
bag, "/poses_acquisition/raw_video/" + ACQ_DEVICE_NAME +
"/compressed")
publisherRectifiedImages = MockupPublisher(
bag, "/poses_acquisition/rectified_video/" + ACQ_DEVICE_NAME +
"/compressed")
publisherCameraInfoRaw = MockupPublisher(
bag, "/poses_acquisition/camera_info_raw/" + ACQ_DEVICE_NAME)
publisherCameraInfoRectified = MockupPublisher(
bag,
"/poses_acquisition/camera_info_rectified/" + ACQ_DEVICE_NAME)
logger.info(
"Setting up the server side process completed. Waiting for messages..."
)
# Run continuously, check for new data arriving from the acquisitionProcessor and processed it when it arrives
while not quitEvent.is_set():
try:
newQueueData = outputDictQueue.get(block=True, timeout=5)
# logger.info("Backlog of messages to send: %d messages" % outputDictQueue.qsize())
incomingData = pickle.loads(newQueueData)
if "apriltags" in incomingData:
for tag in incomingData["apriltags"]:
# Publish the relative pose
newApriltagDetectionMsg = AprilTagDetection()
newApriltagDetectionMsg.header.seq = seq_stamper
newApriltagDetectionMsg.header.stamp.secs = int(
tag["timestamp_secs"])
newApriltagDetectionMsg.header.stamp.nsecs = int(
tag["timestamp_nsecs"])
newApriltagDetectionMsg.header.frame_id = str(
tag["source"])
newApriltagDetectionMsg.transform.translation.x = float(
tag["tvec"][0])
newApriltagDetectionMsg.transform.translation.y = float(
tag["tvec"][1])
newApriltagDetectionMsg.transform.translation.z = float(
tag["tvec"][2])
newApriltagDetectionMsg.transform.rotation.x = float(
tag["qvec"][0])
newApriltagDetectionMsg.transform.rotation.y = float(
tag["qvec"][1])
newApriltagDetectionMsg.transform.rotation.z = float(
tag["qvec"][2])
newApriltagDetectionMsg.transform.rotation.w = float(
tag["qvec"][3])
newApriltagDetectionMsg.tag_id = int(tag["tag_id"])
newApriltagDetectionMsg.tag_family = tag["tag_family"]
newApriltagDetectionMsg.hamming = int(tag["hamming"])
newApriltagDetectionMsg.decision_margin = float(
tag["decision_margin"])
newApriltagDetectionMsg.homography = tag[
"homography"].flatten()
newApriltagDetectionMsg.center = tag["center"]
newApriltagDetectionMsg.corners = tag["corners"].flatten()
newApriltagDetectionMsg.pose_error = tag["pose_error"]
publisherPoses.publish(newApriltagDetectionMsg)
if mode == "live":
logger.info(
"Published pose for tag %d in sequence %d" %
(tag["tag_id"], seq_stamper))
counts[newApriltagDetectionMsg.tag_id] += 1
if "odometry" in incomingData:
publisherOdometry.publish(incomingData["odometry"])
counts["odometry"] += 1
# Publish the test and raw data if submitted and requested:
if ACQ_TEST_STREAM:
if "test_stream_image" in incomingData:
imgMsg = incomingData["test_stream_image"]
imgMsg.header.seq = seq_stamper
publisherTestImages.publish(imgMsg)
if "raw_image" in incomingData:
imgMsg = incomingData["raw_image"]
imgMsg.header.seq = seq_stamper
publisherRawImages.publish(imgMsg)
if "rectified_image" in incomingData:
imgMsg = incomingData["rectified_image"]
imgMsg.header.seq = seq_stamper
publisherRectifiedImages.publish(imgMsg)
if "raw_camera_info" in incomingData:
publisherCameraInfoRaw.publish(
incomingData["raw_camera_info"])
if "new_camera_matrix" in incomingData:
new_camera_info = CameraInfo()
try:
new_camera_info.header = incomingData[
"raw_camera_info"].header
new_camera_info.height = incomingData[
"raw_image"].shape[0]
new_camera_info.width = incomingData[
"raw_image"].shape[1]
new_camera_info.distortion_model = incomingData[
"raw_camera_info"].distortion_model
new_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
except:
pass
new_camera_info.K = incomingData[
"new_camera_matrix"].flatten()
publisherCameraInfoRectified.publish(new_camera_info)
seq_stamper += 1
except KeyboardInterrupt:
raise (Exception("Exiting"))
except Queue.Empty:
if os.getenv('ACQ_DEVICE_MODE', 'live') == 'live':
logger.warning("No messages received in the last 5 seconds!")
except Exception as e:
logger.warning("Exception: %s" % str(e))
pass
# Close the bag (postprocessing mode only)
if mode == 'postprocessing':
bag.close()
# Save or append to an existing file:
if ACQ_OBSERVATIONS_STATISTICS_OUTPUT:
logger.info("Saving statistics to %s",
ACQ_OBSERVATIONS_STATISTICS_OUTPUT)
new_stats = dict()
for id, count in counts.iteritems():
new_stats[id] = count
yaml.dump({ACQ_DEVICE_NAME: new_stats},
open(ACQ_OBSERVATIONS_STATISTICS_OUTPUT, 'a'))
# print the observation statistics in the terminal
logger.info("\n\n")
logger.info("----------------------------------------------------")
logger.info("COUNTS OF OBSERVED APRIL TAGS AND ODOMETRY MESSAGES:")
logger.info("Device: %s" % ACQ_DEVICE_NAME)
for id, count in counts.iteritems():
logger.info("tag: {0}\t{1} observations".format(id, count))
def publishOnServer(outputDictQueue, quitEvent):
"""
Publishes the processed data on the ROS Master that the graph optimizer uses.
"""
# Get the environment variables
ACQ_POSES_TOPIC = os.getenv('ACQ_POSES_TOPIC', "poses")
ACQ_ODOMETRY_TOPIC = os.getenv('ACQ_ODOMETRY_TOPIC', "odometry")
ACQ_DEVICE_NAME = os.getenv('ACQ_DEVICE_NAME', "watchtower10")
ACQ_TEST_STREAM = bool(int(os.getenv('ACQ_TEST_STREAM', 1)))
seq_stamper = 0
publisherPoses = rospy.Publisher("/poses_acquisition/" + ACQ_POSES_TOPIC,
AprilTagDetection,
queue_size=1)
publisherOdometry = rospy.Publisher("/poses_acquisition/" +
ACQ_ODOMETRY_TOPIC,
TransformStamped,
queue_size=1)
# If the test stream is requested
if ACQ_TEST_STREAM:
publisherTestImages = rospy.Publisher(
"/poses_acquisition/test_video/" + ACQ_DEVICE_NAME + "/compressed",
CompressedImage,
queue_size=1)
publisherRawImages = rospy.Publisher("/poses_acquisition/raw_video/" +
ACQ_DEVICE_NAME + "/compressed",
CompressedImage,
queue_size=1)
publisherRectifiedImages = rospy.Publisher(
"/poses_acquisition/rectified_video/" + ACQ_DEVICE_NAME +
"/compressed",
CompressedImage,
queue_size=1)
publisherCameraInfoRaw = rospy.Publisher(
"/poses_acquisition/camera_info_raw/" + ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
publisherCameraInfoRectified = rospy.Publisher(
"/poses_acquisition/camera_info_rectified/" + ACQ_DEVICE_NAME,
CameraInfo,
queue_size=1)
rospy.init_node('acquisition_node_' + ACQ_DEVICE_NAME)
# Run continuously, check for new data arriving from the acquisitionProcessor and processed it when it arrives
while not quitEvent.is_set():
try:
newQueueData = outputDictQueue.get(block=True, timeout=1)
incomingData = pickle.loads(newQueueData)
print("Backlog of messages to send: %d messages" %
outputDictQueue.qsize())
if "apriltags" in incomingData:
for tag in incomingData["apriltags"]:
# Publish the relative pose
newApriltagDetectionMsg = AprilTagDetection()
newApriltagDetectionMsg.header.seq = seq_stamper
newApriltagDetectionMsg.header.stamp.secs = int(
tag["timestamp_secs"])
newApriltagDetectionMsg.header.stamp.nsecs = int(
tag["timestamp_nsecs"])
newApriltagDetectionMsg.header.frame_id = str(
tag["source"])
newApriltagDetectionMsg.transform.translation.x = float(
tag["tvec"][0])
newApriltagDetectionMsg.transform.translation.y = float(
tag["tvec"][1])
newApriltagDetectionMsg.transform.translation.z = float(
tag["tvec"][2])
newApriltagDetectionMsg.transform.rotation.x = float(
tag["qvec"][0])
newApriltagDetectionMsg.transform.rotation.y = float(
tag["qvec"][1])
newApriltagDetectionMsg.transform.rotation.z = float(
tag["qvec"][2])
newApriltagDetectionMsg.transform.rotation.w = float(
tag["qvec"][3])
newApriltagDetectionMsg.tag_id = int(tag["tag_id"])
newApriltagDetectionMsg.tag_family = tag["tag_family"]
newApriltagDetectionMsg.hamming = int(tag["hamming"])
newApriltagDetectionMsg.decision_margin = float(
tag["decision_margin"])
newApriltagDetectionMsg.homography = tag[
"homography"].flatten()
newApriltagDetectionMsg.center = tag["center"]
newApriltagDetectionMsg.corners = tag["corners"].flatten()
newApriltagDetectionMsg.pose_error = tag["pose_error"]
publisherPoses.publish(newApriltagDetectionMsg)
print("Published pose for tag %d in sequence %d" %
(tag["tag_id"], seq_stamper))
if "odometry" in incomingData:
publisherOdometry.publish(incomingData["odometry"])
# Publish the test and raw data if submitted and requested:
if ACQ_TEST_STREAM:
if "test_stream_image" in incomingData:
imgMsg = incomingData["test_stream_image"]
imgMsg.header.seq = seq_stamper
publisherTestImages.publish(imgMsg)
if "raw_image" in incomingData:
imgMsg = incomingData["raw_image"]
imgMsg.header.seq = seq_stamper
publisherRawImages.publish(imgMsg)
if "rectified_image" in incomingData:
imgMsg = incomingData["rectified_image"]
imgMsg.header.seq = seq_stamper
publisherRectifiedImages.publish(imgMsg)
if "raw_camera_info" in incomingData:
publisherCameraInfoRaw.publish(
incomingData["raw_camera_info"])
if "rectified_camera_info" in incomingData:
publisherCameraInfoRectified.publish(
incomingData["rectified_camera_info"])
seq_stamper += 1
except KeyboardInterrupt:
raise (Exception("Exiting"))
except Exception as e:
print("Exception: %s" % str(e))
pass
def publish(self, incomingData):
if "apriltags" in incomingData:
for tag in incomingData["apriltags"]:
# Publish the relative pose
newApriltagDetectionMsg = AprilTagDetection()
newApriltagDetectionMsg.header.stamp.secs = int(
tag["timestamp_secs"])
newApriltagDetectionMsg.header.stamp.nsecs = int(
tag["timestamp_nsecs"])
newApriltagDetectionMsg.header.frame_id = str(tag["source"])
newApriltagDetectionMsg.transform.translation.x = float(
tag["tvec"][0])
newApriltagDetectionMsg.transform.translation.y = float(
tag["tvec"][1])
newApriltagDetectionMsg.transform.translation.z = float(
tag["tvec"][2])
newApriltagDetectionMsg.transform.rotation.x = float(
tag["qvec"][0])
newApriltagDetectionMsg.transform.rotation.y = float(
tag["qvec"][1])
newApriltagDetectionMsg.transform.rotation.z = float(
tag["qvec"][2])
newApriltagDetectionMsg.transform.rotation.w = float(
tag["qvec"][3])
newApriltagDetectionMsg.tag_id = int(tag["tag_id"])
newApriltagDetectionMsg.tag_family = tag["tag_family"]
newApriltagDetectionMsg.hamming = int(tag["hamming"])
newApriltagDetectionMsg.decision_margin = float(
tag["decision_margin"])
newApriltagDetectionMsg.homography = tag["homography"].flatten(
)
newApriltagDetectionMsg.center = tag["center"]
newApriltagDetectionMsg.corners = tag["corners"].flatten()
newApriltagDetectionMsg.pose_error = tag["pose_error"]
self.publish_queue.put({
"topic": "apriltags",
"message": newApriltagDetectionMsg
})
# self.publishers["apriltags"].publish(newApriltagDetectionMsg)
self.logger.info("Published pose for tag %d in sequence %d" %
(tag["tag_id"], self.seq_stamper))
# Publish the test and raw data if submitted and requested:
if self.ACQ_TEST_STREAM:
if "test_stream_image" in incomingData:
imgMsg = incomingData["test_stream_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "test_stream_image",
"message": imgMsg
})
# self.publishers["test_stream_image"].publish(imgMsg)
if "raw_image" in incomingData:
imgMsg = incomingData["raw_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "raw_image",
"message": imgMsg
})
# self.publishers["raw_image"].publish(imgMsg)
if "rectified_image" in incomingData:
imgMsg = incomingData["rectified_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "rectified_image",
"message": imgMsg
})
# self.publishers["rectified_image"].publish(imgMsg)
if "raw_camera_info" in incomingData:
self.publish_queue.put({
"topic":
"raw_camera_info",
"message":
incomingData["raw_camera_info"]
})
# self.publishers["raw_camera_info"].publish(
# incomingData["raw_camera_info"])
if "new_camera_matrix" in incomingData:
new_camera_info = CameraInfo()
try:
new_camera_info.header = incomingData[
"raw_camera_info"].header
new_camera_info.height = incomingData["raw_image"].shape[0]
new_camera_info.width = incomingData["raw_image"].shape[1]
new_camera_info.distortion_model = incomingData[
"raw_camera_info"].distortion_model
new_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
except:
pass
new_camera_info.K = incomingData["new_camera_matrix"].flatten()
self.publish_queue.put({
"topic": "new_camera_matrix",
"message": new_camera_info
})