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 process_measurements(self, goal):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and estimation
are done.
"""
self._apriltag_localization.broadcast_static_tf()
odom_meas = self._base_controller.get_measurement() # current wheel angles (wheel_angle_left, wheel_angle_right)
tag_meas = self._apriltag_localization.get_measurement() # calculated robot pose(s) based on Apriltag detections (x, y, theta)
# Debug
# print 'wheel_angle_left = ', self._base_controller._wheel_angle_left
# print 'wheel_angle_right = ', self._base_controller._wheel_angle_right
# print 'odom_meas = ', odom_meas
# print 'tag_meas = ', tag_meas
# odom_meas = None
# tag_meas = None
# Do KalmanFilter step
pose_est = self._kalman_filter.step_filter(odom_meas, tag_meas)
# print 'pose_est = ', pose_est
# print 'goal = ', goal
poselist_map2base_ekf = [pose_est[0], pose_est[1], 0, 0, 0, pose_est[2]]
pubFrame(self._br, pose = poselist_map2base_ekf, frame_id = '/base_link_ekf', parent_frame_id = '/map')
at_goal = False
v, omega, at_goal = self._diff_drive_controller.compute_vel(pose_est, goal)
cmd_vel_msg = Twist()
cmd_vel_msg.linear.x = v
cmd_vel_msg.angular.z = omega
# self._cmd_vel_pub.publish(cmd_vel_msg)
# self._base_controller.command_velocity(v, omega)
return at_goal
def main():
rospy.init_node('apriltag_navi', anonymous=True)
lr = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
poselist_tag_map = [0,0,0.44,0.5,0.5,0.5,0.5]
print 'poselist_tag_map', poselist_tag_map, '\n'
pose_tag_map = poselist2pose(poselist_tag_map)
print 'poselist2pose(poselist_tag_map):\n', pose_tag_map, '\n'
poselist_map_tag = invPoselist(poselist_tag_map)
print 'invPoselist(poselist_tag_map):', poselist_map_tag, '\n'
poselist_tag_map_bylookup = lookupTransform(lr, sourceFrame = '/apriltag', targetFrame = '/map')
print "lookupTransform(poselist_tag_map, sourceFrame = '/tag', targetFrame = '/map'):", poselist_tag_map_bylookup, '\n'
poselist_map_tag_bylookup = lookupTransform(lr, sourceFrame = '/map', targetFrame = '/apriltag')
print "lookupTransform(poselist_tag_map, sourceFrame = '/map', targetFrame = '/apriltag'):", poselist_map_tag_bylookup, '\n'
poselist_base_tag = [0,0,1,0,0,0,1]
poselist_base_map = transformPose(lr, poselist_base_tag, sourceFrame = '/apriltag', targetFrame = '/map')
print "transformPose(poselist_tag_map, sourceFrame = '/apriltag', targetFrame = '/map'):", poselist_base_map, '\n'
for i in xrange(100):
pubFrame(br, pose = poselist_base_map, frame_id = '/robot_base', parent_frame_id = '/map')
rospy.sleep(0.1)
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 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.transforms) == 0):
self._pose_detections = None
self._tag_pose_pub.publish(pose_array_msg)
return
pose_detections = np.zeros((len(pose_array.transforms), 3))
for i in range(len(pose_array.transforms)):
pose_msg = Pose()
tag_id = pose_array.transforms[i].fiducial_id
transform_cam2tag = pose_array.transforms[i].transform
# print "transform_cam2tag = ", transform_cam2tag
poselist_cam2tag = transform2poselist(transform_cam2tag)
poselist_base2tag = transformPose(self._lr, poselist_cam2tag,
'camera', 'base_link')
poselist_tag2base = invPoselist(poselist_base2tag)
# print "poselist_tag2base = ", poselist_tag2base
poselist_map2base = transformPose(self._lr, poselist_tag2base,
'apriltag_' + str(tag_id),
'map')
# print "poselist_map2base = ", poselist_map2base
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_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
det_poselist = helper.pose2poselist(detection.pose.pose)
basetag_poselist = transformPose(lr, det_poselist, '/camera',
'/robot_base')
basetag_poselist_inv = invPoselist(basetag_poselist)
mapbase_poselist = transformPose(lr, basetag_poselist_inv,
'/apriltag', '/map')
mapbase_pose = poselist2pose(mapbase_poselist)
pubFrame(br, mapbase_pose)
def broadcast_static_tf(self):
"""
Broadcast static transforms (camera pose in base_link frame, all tag poses in map frame)
"""
pubFrame(self._br,
pose=self._poselist_base2cam,
frame_id='/camera',
parent_frame_id='/base_link')
for tag_id, poselist_map2tag in self._tag_poses.iteritems():
pubFrame(self._br,
pose=poselist_map2tag,
frame_id='/apriltag_' + str(tag_id),
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:
##
poselist_tag_cam = pose2list(detection.pose)
poselist_tag_base = poseTransform(poselist_tag_cam,
homeFrame='/camera',
targetFrame='/robot_base',
listener=lr)
poselist_base_tag = invPoseList(poselist_tag_base)
poselist_base_map = poseTransform(poselist_base_tag,
homeFrame='/apriltag0',
targetFrame='/map',
listener=lr)
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:
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(self, data):
# use apriltag pose detection to find where is the robot
##
for detection in data.detections:
if detection.id in range(11):
pose_tag_base = helper.poseTransform(helper.pose2list(
detection.pose),
homeFrame='/camera',
targetFrame='/robot_base',
listener=self.listener)
pose_base_map = helper.poseTransform(
helper.invPoseList(pose_tag_base),
homeFrame='/apriltag' + str(detection.id),
targetFrame='/map',
listener=self.listener)
helper.pubFrame(self.br,
pose=pose_base_map,
frame_id='/robot_base',
parent_frame_id='/map',
npub=1)
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: # 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(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):
global lr, br
global apriltag_number
# use apriltag pose detection to find where is the robot
for detection in data.detections:
if detection.id == apriltag_number:
tagframe = '/apriltag%d' % detection.id
##
poselist_tag_cam = pose2list(detection.pose)
poselist_tag_base = poseTransform(poselist_tag_cam,
homeFrame='/camera',
targetFrame='/robot_base',
listener=lr)
poselist_base_tag = invPoseList(poselist_tag_base)
poselist_base_map = poseTransform(poselist_base_tag,
homeFrame=tagframe,
targetFrame='/map',
listener=lr)
pubFrame(br,
pose=poselist_base_map,
frame_id='/robot_base',
parent_frame_id='/map')
print apriltag_number
