Go to the documentation of this file.00001
00002
00003
00004
00005
00006 import rospy
00007 import tf
00008 import ar_track_alvar_msgs.msg as ar_track_alvar_msgs
00009 import yocs_msgs.msg as yocs_msgs
00010 import copy
00011 import math
00012
00013 def remove_leading_slash(frame_id):
00014 if frame_id.startswith('/'):
00015 return frame_id[1:]
00016 else:
00017 return frame_id
00018
00019 class TrackerManager(object):
00020 """
00021 Receives AR Pair Annotations from annotation server.
00022 Configure yocs_ar_pair_tracking and publish TFs
00023 """
00024 def __init__(self):
00025 self._init_params()
00026 self._init_variables()
00027
00028 self._sub_global_pairs = rospy.Subscriber('global_pairs', ar_track_alvar_msgs.AlvarMarkers, self._process_global_pairs)
00029 self._pub_pair_list = rospy.Publisher('update_ar_pairs', yocs_msgs.ARPairList, latch=True, queue_size=1)
00030
00031
00032 def _init_params(self):
00033 param = {}
00034 param['tf_broadcast_freq'] = rospy.get_param('tf_broadcast_freq', 0.5)
00035 param['ar_pair_baseline'] = rospy.get_param('ar_pair/baseline', 0.28)
00036 param['ar_pair_target_offset'] = rospy.get_param('ar_pair/target_offset', 0.5)
00037 param['ar_pair_global_prefix'] = rospy.get_param('ar_pair/global_prefix', 'global_marker')
00038 param['ar_pair_target_postfix'] = rospy.get_param('ar_pair/global_postfix', 'target')
00039
00040 self.loginfo("Target offset : %s"%param['ar_pair_target_offset'])
00041
00042 self.param = param
00043
00044 def _init_variables(self):
00045 self._tf_broadcaster = tf.TransformBroadcaster()
00046 self._global_pairs = []
00047
00048 def _process_global_pairs(self, msg):
00049
00050 self._global_pairs = copy.deepcopy(msg.markers)
00051 self.loginfo('Received ' + str(len(self._global_pairs)) + ' markers')
00052
00053
00054 self._notify_ar_pair_tracker()
00055
00056 def _notify_ar_pair_tracker(self):
00057 pair_list = yocs_msgs.ARPairList()
00058
00059 baseline = self.param['ar_pair_baseline']
00060 target_offset = self.param['ar_pair_target_offset']
00061 global_prefix = self.param['ar_pair_global_prefix']
00062 target_postfix = self.param['ar_pair_target_postfix']
00063
00064 for marker in self._global_pairs:
00065 p = yocs_msgs.ARPair()
00066 p.left_id = marker.id
00067 p.right_id = p.left_id - 3
00068 p.baseline = baseline
00069 p.target_offset = target_offset
00070 p.target_frame = global_prefix + '_' + str(marker.id) + '_' + target_postfix
00071 pair_list.pairs.append(p)
00072
00073 self._pub_pair_list.publish(pair_list)
00074
00075
00076
00077 def _publish_target_tfs(self):
00078 global_prefix = self.param['ar_pair_global_prefix']
00079 target_postfix = self.param['ar_pair_target_postfix']
00080 target_offset = self.param['ar_pair_target_offset']
00081
00082 for marker in self._global_pairs:
00083 parent_frame_id = global_prefix + '_' + str(marker.id)
00084 child_frame_id = parent_frame_id + '_' + target_postfix
00085 parent_frame_id = remove_leading_slash(parent_frame_id)
00086 child_frame_id = remove_leading_slash(child_frame_id)
00087
00088 p = (0,0, target_offset)
00089 q = tf.transformations.quaternion_from_euler(math.pi, 0, 0)
00090 self._tf_broadcaster.sendTransform(p, q, rospy.Time.now(), child_frame_id , parent_frame_id)
00091 rospy.sleep(0.05)
00092
00093 def _publish_marker_tfs(self):
00094 global_prefix = self.param['ar_pair_global_prefix']
00095 for marker in self._global_pairs:
00096 parent_frame_id = marker.pose.header.frame_id
00097 child_frame_id = global_prefix + '_' + str(marker.id)
00098 parent_frame_id = remove_leading_slash(parent_frame_id)
00099 child_frame_id = remove_leading_slash(child_frame_id)
00100 p = (marker.pose.pose.position.x,marker.pose.pose.position.y, marker.pose.pose.position.z)
00101 q = (marker.pose.pose.orientation.x, marker.pose.pose.orientation.y, marker.pose.pose.orientation.z,marker.pose.pose.orientation.w)
00102 self._tf_broadcaster.sendTransform(p, q, rospy.Time.now(), child_frame_id ,parent_frame_id)
00103 rospy.sleep(0.05)
00104
00105 def loginfo(self, msg):
00106 rospy.loginfo('TrackerManager : ' + str(msg))
00107
00108 def spin(self):
00109 if self.param['tf_broadcast_freq'] == 0:
00110 rospy.spin()
00111 else:
00112 hertz = 1 / self.param['tf_broadcast_freq']
00113 r = rospy.Rate(hertz)
00114 while not rospy.is_shutdown():
00115 self._publish_marker_tfs()
00116 r.sleep()
00117 self._publish_target_tfs()
00118 r.sleep()