def apriltag_callback(self, data):
# use apriltag pose detection to find where is the robot
# print data
for detection in data.detections:
if self.idx == 6 and detection.id == 12:
self.idx = self.idx + 1
if (self.idx == 5 and detection.id == 4) and (
self.idx == 8
and detection.id == 7): # tag id is the correct one
self.detection_id = detection.id
poselist_tag_cam = pose2poselist(detection.pose)
poselist_tag_base = transformPose(self.lr,
poselist_tag_cam,
sourceFrame='/camera',
targetFrame='/robot_base')
poselist_base_tag = invPoselist(poselist_tag_base)
poselist_base_map = transformPose(self.lr,
poselist_base_tag,
sourceFrame='/apriltag',
targetFrame='/map')
pubFrame(self.br,
pose=poselist_base_map,
frame_id='/robot_base',
parent_frame_id='/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
for detection in data.detections:
if detection.id == 0: #FIX THIS: change the number to equal the id on the april tag you're using
poselist_tag_cam = pose2poselist(detection.pose) #This is the pose you're transforming
## Relevant frames that are already defined:
## camera, map, robot_base, apriltag
## Relevant functions that are defined in a different ros package:
## transformPose(), invPoseList()
## Relevant variables already defined:
## lr
#TO DO: Convert the poselist_tag_cam into the pose that we want.
#HINT: The first argument for transformPose is lr which is already defined in the beginning of this code.
#HINT2: The syntax for transformPose should look like:
## transformPose(lr, poselist_someframe_otherframe, 'otherframe', 'newframe')\
#HINT3: The syntax for invPoselist should look like:
## invPoselist(poselist)
## poselist_tag_base = transformPose(??) #(1) on the lab handout
## poselist_base_tag = invPoselist(??) #(2) on the lab handout
## poselist_base_map = transformPose(??) #(3) on the lab handout
pubFrame(br, pose = poselist_base_map, frame_id = '/robot_base', parent_frame_id = '/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
if len(data.detections) != 0: # check if apriltag is detected
detection = data.detections[0]
print detection.pose
if detection.id == 21:
print "detection"
pose = pose2poselist(detection.pose.pose)
base_to_tag = transformPose(lr,
pose,
sourceFrame='/camera',
targetFrame='/apriltag')
tag_to_base = invPoselist(base_to_tag)
map_to_base = transformPose(lr,
tag_to_base,
sourceFrame='/map',
targetFrame='/base_link')
# tag id is the correct one
# Use the functions in helper.py to do the following
# step 1. convert the pose to poselist Hint: pose data => detection.pose.pose
#poselist = pose2poselist(detection.pose.pose)
# step 2. do the matrix manipulation
#mm = transformPose(lr, poselist, 21, detection.id)
# step 3. publish the base frame w.r.t the map frame
br.w = map_to_base[6]
br.r = map_to_base[3:5]
br.t = map_to_base[0:2]
def april_tag_callback(data, tag_id):
state.tag_time = data.header.stamp.to_sec()
data = pose2poselist(data.pose)
x, y = data[0:2]
theta = tfm.euler_from_quaternion(data[3:7])[2]
state.tag_data[tag_id] = [x, y, theta]
#state.tag_measure_time=data.header.stamp.to_sec()
pass
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
for detection in data.detections:
if detection.id == 0: # tag id is the correct one
poselist_tag_cam = pose2poselist(detection.pose)
poselist_tag_base = transformPose(lr, poselist_tag_cam, sourceFrame = '/camera', targetFrame = '/robot_base')
poselist_base_tag = invPoselist(poselist_tag_base)
poselist_base_map = transformPose(lr, poselist_base_tag, sourceFrame = '/apriltag', targetFrame = '/map')
pubFrame(br, pose = poselist_base_map, frame_id = '/robot_base', parent_frame_id = '/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
for detection in data.detections:
if detection.id == 0: # tag id is the correct one
poselist_tag_cam = pose2poselist(detection.pose)
## poselist_tag_base = transformPose(??)
## poselist_base_tag = invPoselist(??)
## poselist_base_map = transformPose(??)
pubFrame(br,
pose=poselist_base_map,
frame_id='/robot_base',
parent_frame_id='/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
if len(data.detections)!=0: # check if apriltag is detected
detection = data.detections[0]
print detection.pose
if detection.id == 21: # tag id is the correct one
print detection
# Use the functions in helper.py to do the following
# step 1. convert the pose to poselist Hint: pose data => detection.pose.pose
tf_apriltag_camera = pose2poselist(detection.pose.pose)
# step 2. do the matrix manipulation
tf_base_map = tf_apriltag_map * np.inv (tf_camera_base * tf_apriltag_map)
def callback(self, pose_array):
"""
Convert pose of tag in camera frame to pose of robot
in map frame.
"""
with self._lock:
pose_array_msg = PoseArray()
# Camera frame to tag frame(s)
if (len(pose_array.detections) == 0):
self._pose_detections = None
self._tag_pose_pub.publish(pose_array_msg)
return
pose_detections = np.zeros((len(pose_array.detections), 3))
for i in range(len(pose_array.detections)):
pose_msg = Pose()
tag_id = pose_array.detections[i].id
pose_cam2tag = pose_array.detections[i].pose.pose
poselist_cam2tag = pose2poselist(pose_cam2tag)
poselist_base2tag = transformPose(self._lr, poselist_cam2tag,
'camera', 'base_link')
poselist_tag2base = invPoselist(poselist_base2tag)
poselist_map2base = transformPose(self._lr, poselist_tag2base,
'apriltag_' + str(tag_id),
'map')
pubFrame(self._br,
pose=poselist_map2base,
frame_id='/base_link',
parent_frame_id='/map')
robot_pose3d = lookupTransform(self._lr, '/map', '/base_link')
robot_position2d = robot_pose3d[0:2]
robot_yaw = tf.transformations.euler_from_quaternion(
robot_pose3d[3:7])[2]
robot_pose2d = robot_position2d + [robot_yaw]
pose_detections[i] = np.array(robot_pose2d)
pose_msg.position.x = robot_pose3d[0]
pose_msg.position.y = robot_pose3d[1]
pose_msg.orientation.x = robot_pose3d[3]
pose_msg.orientation.y = robot_pose3d[4]
pose_msg.orientation.z = robot_pose3d[5]
pose_msg.orientation.w = robot_pose3d[6]
pose_array_msg.poses.append(pose_msg)
self._tag_pose_pub.publish(pose_array_msg)
self._pose_detections = pose_detections
def apriltag_cb(self, msg):
try:
if self.done: return
except:
return
# use apriltag pose detection to find where is the robot
for detection in msg.detections:
if detection.id[0] == 10: # tag id is the correct one
poselist_tag_cam = pose2poselist(detection.pose.pose.pose)
euler = tfm.euler_from_quaternion(poselist_tag_cam[3:7])
print(euler)
self.avg = (self.count * self.avg +
euler[1]) * 1. / (self.count + 1)
self.count += 1
if self.count > 10:
self.done = True
self.angle_pub.publish(self.avg)
print "Published camera angle", self.avg
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
for detection in data.detections:
id = detection.id
if ((id == 1) or (id == 0) or (id == 2) or
(id
== 3)): # tag id is one the correct ones that are on game board
#somewhere in here we need to add april tag location relative to map
poselist_tag_cam = pose2poselist(detection.pose)
poselist_tag_base = transformPose(lr, poselist_tag_cam, 'camera',
'robot_base')
poselist_base_tag = invPoselist(poselist_tag_base)
poselist_base_map = transformPose(lr, poselist_base_tag,
'apriltag' + str(id), 'map')
pubFrame(br,
pose=poselist_base_map,
frame_id='/robot_base_est',
parent_frame_id='/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
if len(data.detections) != 0: # check if apriltag is detected
detection = data.detections[0]
print detection.pose
if detection.id == 21:
print 'detection'
# tag id is the correct one
# Use the functions in helper.py to do the following
# step 1. convert the pose to poselist Hint: pose data => detection.pose.pose
A = pose2poselist(detection.pose.pose)
# step 2. do the matrix manipulation
# A source B target transformPose(,pose,B,A)
A = transformPose(lr, A, '/camera', '/base_link')
A = invPoselist(A)
A = transformPose(lr, A, '/apriltag', '/map')
pubFrame(br, A, '/base_link', '/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
if len(data.detections) != 0: # check if apriltag is detected
detection = data.detections[0]
print detection.pose
if detection.id == 20: # tag id is the correct one
# Use the functions in helper.py to do the following
# step 1. convert the pose to poselist Hint: pose data => detection.pose.pose
# step 2. do the matrix manipulation
# step 3. publish the base frame w.r.t the map frame
# note: tf listener and broadcaster are initalize in line 19~20
poselist1 = pose2poselist(detection.pose.pose)
poselist1_T2B = transformPose(lr, poselist1, '/camera',
'/base_link')
poselist1_T2B_inv = invPoselist(poselist1_T2B)
poselist1_B2M = transformPose(lr, poselist1_T2B_inv, '/apriltag',
'map')
#detection.pose.pose = poselist2pose(poselist1_B2M) #detection is form camera so no need to write back
pubFrame(br, poselist1_B2M)
def apriltag_callback(self, data):
# use apriltag pose detection to find where is the robot
# print data
for detection in data.detections:
print 'apriltag id', detection.id
poselist_tag_cam = pose2poselist(detection.pose)
poselist_tag_base = transformPose(self.lr,
poselist_tag_cam,
sourceFrame='/camera',
targetFrame='/robot_base')
poselist_base_tag = invPoselist(poselist_tag_base)
poselist_base_map = transformPose(self.lr,
poselist_base_tag,
sourceFrame='/apriltag',
targetFrame='/map')
pubFrame(self.br,
pose=poselist_base_map,
frame_id='/robot_base',
parent_frame_id='/map')
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
if len(data.detections) != 0:
detection = data.detections[0]
print detection.pose
if detection.id == 20: # tag id is the correct one
poselist_tag_cam = pose2poselist(detection.pose.pose)
poselist_tag_base = transformPose(lr,
poselist_tag_cam,
sourceFrame='camera',
targetFrame='base_link')
print poselist_tag_base
poselist_base_tag = invPoselist(poselist_tag_base)
poselist_base_map = transformPose(lr,
poselist_base_tag,
sourceFrame='apriltag',
targetFrame='map')
pubFrame(br,
pose=poselist_base_map,
frame_id='/base_link',
parent_frame_id='/map')
def apriltag_callback(data):
tt=rospy.Time.now()
# use apriltag pose detection to find where is the robot
for detection in data.detections:
#if detection.id == 1: # tag id is the correct one
poselist_tag_cam = pose2poselist(detection.pose)
#print np.sqrt(poselist_tag_cam[0]**2+poselist_tag_cam[1]**2+poselist_tag_cam[2])
#print poselist_tag_cam[0:3]
#poselist_tag_cam[0]=poselist_tag_cam[0]*TAG_CONSTANT
#poselist_tag_cam[1]=poselist_tag_cam[1]*TAG_CONSTANT
#poselist_tag_cam[2]=poselist_tag_cam[2]*TAG_CONSTANT
print poselist_tag_cam[0:3]
poselist_tag_base = transformPose(lr, poselist_tag_cam, 'camera', 'robot_base')
print poselist_tag_base[0:3]
#print poselist_tag_base
#print type(detection.id)
tag_pose=poselist2pose(poselist_tag_base)
if detection.id in tag_id_list:
pose_stamp=PoseStamped()
pose_stamp.header.stamp = tt
pose_stamp.header.frame_id = "/base"
pose_stamp.pose=tag_pose
tag_pubs[detection.id].publish(pose_stamp)
