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(self, data):
# use apriltag pose detection to find where is the robot
##
for detection in data.detections:
if detection.id == 0:
pose_tag_base = helper.poseTransform(helper.pose2list(
detection.pose),
homeFrame='/camera',
targetFrame='/base_link',
listener=self.listener)
pose_base_map = helper.poseTransform(
helper.invPoseList(pose_tag_base),
homeFrame='/apriltag',
targetFrame='/map',
listener=self.listener)
pubFrame(self.br,
pose=pose_base_map,
frame_id='/base_link',
parent_frame_id='/map',
npub=1)
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