def talker(self):
pub = rospy.Publisher('tag_detections_merged', AprilTagDetectionArray, queue_size=10)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
detection_array = AprilTagDetectionArray()
for tag in self.tag_list:
detections_from_cameras = []
for topic in self.camera_data.keys():
if (tag in self.camera_data[topic].keys()):
if (self.camera_data[topic][tag] != None):
detections_from_cameras.append(self.camera_data[topic][tag])
self.camera_data[topic][tag] = None
if (len(detections_from_cameras)>0):
merged_detection = AprilTagDetection()
merged_detection.id = tag
merged_detection.size = detections_from_cameras[0].size
merged_detection.pose.header = detections_from_cameras[0].pose.header
pose_list = [d.pose.pose for d in detections_from_cameras]
merged_detection.pose.pose = self.averagePose (pose_list)
detection_array.detections.append(merged_detection)
pub.publish(detection_array)
rate.sleep()
def maketag(
xIn, yIn, zIn
): #to use, do something like tagCoords = getAprilTagDetection(arguments here); myTag = maketag(tagCoords[0], tagCoords[1], tagCoords[2])
h = std_msgs.msg.Header() #std_msgs.msg.
h.stamp = rospy.Time.now(
) # Note you need to call rospy.init_node() before this will work
p = Point() #geometry_msgs.
p.x = xIn
p.y = yIn
p.z = zIn
q = Quaternion()
q.x = 0
q.y = 0
q.z = 0
q.w = 0
poseU = Pose(p, q)
poseS = PoseStamped(h, poseU)
tag = AprilTagDetection()
tag.size = 0.163513 #default from the launch file
tag.id = 1 #the tad number I believe
tag.pose = poseS
atda = AprilTagDetectionArray()
atda.detections = [tag]
return atda
def process_tag_pose(self):
''' Get the position and orientation of detected april tags from the iOS device. '''
for tags in self.april_tags:
ar = tags.split(",")
current_tag = AprilTagDetection()
current_tag.id = int(ar[0])
current_tag.size = 0.165
current_tag.pose.header.stamp = rospy.Time(float(self.ios_clock_offset) + float(ar[17]))
current_tag.pose.header.frame_id = "camera"
tag = [float(x) for x in ar[1:17]]
mat = np.matrix([tag[0:4], tag[4:8], tag[8:12], tag[12:16]])
new_mat = mat.A
trans = translation_from_matrix(new_mat)
quat = quaternion_from_matrix(new_mat)
current_tag.pose.pose.position.x = trans[0]
current_tag.pose.pose.position.y = trans[1]
current_tag.pose.pose.position.z = trans[2]
current_tag.pose.pose.orientation.x = quat[0]
current_tag.pose.pose.orientation.y = quat[1]
current_tag.pose.pose.orientation.z = quat[2]
current_tag.pose.pose.orientation.w = quat[3]
self.msg.detections.append(current_tag)
def maketagSpecifyHeader(x, y, z, h):
p = Point() #geometry_msgs.
p.x = xIn
p.y = yIn
p.z = zIn
q = Quaternion()
q.x = 0
q.y = 0
q.z = 0
q.w = 0
poseU = Pose(p, q)
poseS = PoseStamped(h, poseU)
tag = AprilTagDetection()
tag.size = 0.163513 #default from the launch file
tad.id = 1 #the tad number I believe
tag.pose = poseS
atda = AprilTagDetectionArray()
atda.detections = [tag]
return atda
