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