system_calibrator.py 文件源码

def __init__(self):
        self.marker_id = rospy.get_param("~marker_id", 12)
        self.frame_id = rospy.get_param("~frame_id", "odom_meas")
        self.camera_frame_id = rospy.get_param("~camera_frame_id", "overhead_cam_frame")
        self.count = rospy.get_param("~count", 50)

        # local vars:
        self.calibrate_flag = False
        self.calibrated = False
        self.calibrate_count = 0
        self.kb = kbhit.KBHit()
        self.trans_arr = np.zeros((self.count, 3))
        self.quat_arr = np.zeros((self.count, 4))
        self.trans_ave = np.zeros(3)
        self.quat_ave = np.zeros(3)

        # now create subscribers, timers, and publishers
        self.br = tf.TransformBroadcaster()
        self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb)
        rospy.on_shutdown(self.kb.set_normal_term)
        self.alvar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.alvarcb)
        return