def callback(data):
global objFramePrefix_
global distanceMax_
if len(data.objects.data) > 0:
output = AprilTagDetectionArray()
output.header = data.header
for i in range(0, len(data.objects.data), 12):
try:
objId = data.objects.data[i]
(trans, quat) = listener.lookupTransform(
data.header.frame_id,
objFramePrefix_ + '_' + str(int(objId)), data.header.stamp)
tag = AprilTagDetection()
tag.id.append(objId)
tag.pose.pose.pose.position.x = trans[0]
tag.pose.pose.pose.position.y = trans[1]
tag.pose.pose.pose.position.z = trans[2]
tag.pose.pose.pose.orientation.x = quat[0]
tag.pose.pose.pose.orientation.y = quat[1]
tag.pose.pose.pose.orientation.z = quat[2]
tag.pose.pose.pose.orientation.w = quat[3]
tag.pose.header = output.header
if distanceMax_ <= 0.0 or trans[2] < distanceMax_:
output.detections.append(tag)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if len(output.detections) > 0:
pub.publish(output)
def __init__(self):
rospy.loginfo("START")
# rospy.on_shutdown(self.shutdown)
self.velocity = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.laser_distance = rospy.Subscriber('scan',
LaserScan,
self.laser_distance_callback,
queue_size=10)
# self.apriltag_image_sub = rospy.Subscriber('/tag_detections_image', Image, self.apriltag_image_callback,queue_size=10)
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw",
Image,
self.image_callback,
queue_size=10)
self.sub = rospy.Subscriber('/tag_detections',
AprilTagDetectionArray,
self.apriltag_callback,
queue_size=10)
self.distance = LaserScan()
# self.tag_image = Image()
self.tag = AprilTagDetectionArray()
self.image = Image()
self.bridge_object = CvBridge()
self.errors = 0
self.error_x = 0
self.forward1 = 0
self.forward2 = 0
def __init__(self):
self.pose_pub = rospy.Publisher("/my_pose",
PoseWithCovarianceStamped,
queue_size=1)
self.sub = rospy.Subscriber('/tag_detections', AprilTagDetectionArray,
self.sub_callback)
self.rawdata = AprilTagDetectionArray()
import rospy
from apriltag_ros.msg import AprilTagDetection, AprilTagDetectionArray
from gazebo_msgs.srv import GetModelState
from geometry_msgs.msg import Point, Point32, Pose, PoseStamped, Quaternion, TransformStamped
from nav_msgs.msg import Odometry
# Mostly Useless Imports
from std_msgs.msg import Header, String, UInt16
import tf
# ---------------------------------------------------------------------------------
size = 0
yoda = Odometry()
tag = 0
sock = AprilTagDetectionArray()
odo_tr = TransformStamped()
flag = [False for i in range(10)]
i=0
# ---------------------------------------------------------------------------------
def callback(data):
global size
global yoda
global tag
global sock
global i
global odo_tr
br = tf.TransformBroadcaster()
sock = data
#!/usr/bin/env python
# -- coding: utf-8 --
import rospy
from apriltag_ros.msg import AprilTagDetection, AprilTagDetectionArray
from geometry_msgs.msg import Point, Point32, Pose, PoseStamped, Quaternion
# Mostly Useless Imports
from std_msgs.msg import Header, String, UInt16
from nav_msgs.msg import Odometry
from gazebo_msgs.srv import GetModelState
# ---------------------------------------------------------------------------------
kolio = AprilTagDetectionArray()
size = 0
yoda = Odometry()
flag = [False for i in range(10)]
# ---------------------------------------------------------------------------------
def callback(data):
print("ZEFT AWY")
global size
global yoda
kolio = data
size = len(kolio.detections)
print("size aho0000" + str(size))
if size > 0:
for i in range(0, size - 1):