def get_current_point(self):
apriltag_source_frame = '/apriltag' + str(self.current_input)
if len(self.prev_pos_x) >= 20:
self.prev_pos_x.pop(0)
self.prev_pos_y.pop(0)
self.prev_yaw.pop(0)
try:
if self.current_input in self.tags_in_view:
poselist_tag_cam = helper.pose2poselist(
self.detection_poses[self.current_input])
pose_tag_base = helper.transformPose(pose=poselist_tag_cam,
sourceFrame='/camera',
targetFrame='/robot_base',
lr=self.listener)
poselist_base_tag = helper.invPoselist(pose_tag_base)
pose_base_map = helper.transformPose(
pose=poselist_base_tag,
sourceFrame=apriltag_source_frame,
targetFrame='/map',
lr=self.listener)
helper.pubFrame(self.br,
pose=pose_base_map,
frame_id='/robot_base',
parent_frame_id='/map',
npub=1)
robot_pose3d = helper.lookupTransform(self.listener, '/map',
'/robot_base')
robot_x = robot_pose3d[0]
robot_y = robot_pose3d[1]
robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
print "filtering", self.filtering_on, "robot_yaw", robot_yaw
should_append = True
if (self.current_input
in self.turn_directions) and self.filtering_on:
if self.turn_directions[
self.current_input] == self.TURN_LEFT:
if robot_yaw <= self.tag_angles[self.current_input]:
should_append = False
elif self.turn_directions[
self.current_input] == self.TURN_RIGHT:
if robot_yaw >= self.tag_angles[self.current_input]:
should_append = False
if should_append:
self.prev_pos_x.append(robot_x)
self.prev_pos_y.append(robot_y)
self.prev_yaw.append(robot_yaw)
self.current_x = sum(self.prev_pos_x) / len(self.prev_pos_x)
self.current_y = sum(self.prev_pos_y) / len(self.prev_pos_y)
self.current_theta = sum(self.prev_yaw) / len(self.prev_yaw)
except:
ex_type, ex, tb = sys.exc_info()
traceback.print_tb(tb)
self.current_x = -1
self.current_y = -1
self.current_theta = -1
print "get_current_point", self.current_x, self.current_y, self.current_theta
def object_callback(self, msg):
# pose of object w.r.t base
object2base = helper.transformPose(self.listener,
helper.pose2poselist(msg),
'/camera', 'base_link')
helper.pubFrame(self.br,
pose=object2base,
frame_id='/object',
parent_frame_id='/base_link',
npub=1)
def frameCallback(msg):
global br
global currentTargetPose, currentTargetPoseDirty
global listener
time = rospy.Time.now()
mutex.acquire()
pubFrame(br,
pose2poselist(currentTargetPose),
'target_pose',
'arm_base',
npub=1)
mutex.release()
def apriltag_callback(self, data):
# use apriltag pose detection to find where is the robot
for detection in data.detections:
if detection.id == self.id:
tag2base = helper.transformPose(
self.listener, helper.pose2poselist(detection.pose),
'/camera', 'base_link')
helper.pubFrame(self.br,
pose=helper.pose2poselist(detection.pose),
frame_id='/apriltag',
parent_frame_id='/camera',
npub=1)
helper.pubFrame(self.br,
pose=tag2base,
frame_id='/apriltag',
parent_frame_id='/base_link',
npub=1)
def apriltag_callback(self, data):
# use apriltag pose detection to find where is the robot
##
for detection in data.detections:
#print detection.pose.position.x, detection.pose.position.y, detection.pose.position.z
if detection.id == 0:
pose_tag_base = helper.transformPose(pose=helper.pose2poselist(
detection.pose),
sourceFrame='/camera',
targetFrame='/robot_base',
lr=self.listener)
pose_base_map = helper.transformPose(
pose=helper.invPoselist(pose_tag_base),
sourceFrame='/apriltag0',
targetFrame='/map',
lr=self.listener)
helper.pubFrame(self.br,
pose=pose_base_map,
frame_id='/robot_base',
parent_frame_id='/map',
npub=1)