mobile_tracker.py 文件源码

def __init__(self):
        # first let's load all parameters:
        self.frame_id = rospy.get_param("~frame_id", "odom_meas")
        self.camera_frame_id = rospy.get_param("~camera_frame_id", "overhead_cam_frame")
        self.base_frame_id = rospy.get_param("~base_frame_id", "base_meas")
        self.x0 = rospy.get_param("~x0", 0.0)
        self.y0 = rospy.get_param("~y0", 0.0)
        self.th0 = rospy.get_param("~th0", 0.0)
        self.pubstate = rospy.get_param("~pubstate", True)
        self.marker_id = rospy.get_param("~marker_id", 12)

        # setup other required vars:
        self.odom_offset = odom_conversions.numpy_to_odom(np.array([self.x0, self.y0, self.th0]),
                                                          self.frame_id)
        self.path_list = deque([], maxlen=PATH_LEN)

        # now let's create publishers, listeners, and subscribers
        self.br = tf.TransformBroadcaster()
        self.listener = tf.TransformListener()
        self.meas_pub = rospy.Publisher("meas_pose", Odometry, queue_size=5)
        self.path_pub = rospy.Publisher("meas_path", Path, queue_size=1)
        self.alvar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.alvarcb)
        self.publish_serv = rospy.Service("publish_bool", SetBool, self.pub_bool_srv_cb)
        self.offset_serv = rospy.Service("set_offset", SetOdomOffset, self.offset_srv_cb)
        return