reference_imu_to_angle.py 文件源码

def __init__(self):
        self.rate = rospy.get_param('~rate', 100.0)
        self.topic_name_angle = rospy.get_param('~topic_name_angle', 'topic_name_angle')
        self.topic_name_reference = rospy.get_param('~topic_name_reference', 'topic_name_reference')
        self.imu_name = rospy.get_param('~imu_name', 'imu_steer')

        self.roll_steer = 0.0
        self.pitch_steer = 0.0
        self.yaw_steer = 0.0

        self.roll_frame = 0.0
        self.pitch_frame = 0.0
        self.yaw_frame = 0.0

        self.enable_reference = False
        self.enable_angle = False

        self.pub_imu_roll_msg = Float32()
        self.pub_imu_pitch_msg = Float32()
        self.pub_imu_yaw_msg = Float32()

        self.pub_imu_roll = rospy.Publisher('/' + self.imu_name +'/roll', Float32, queue_size=1)
        self.pub_imu_pitch = rospy.Publisher('/' + self.imu_name +'/pitch', Float32, queue_size=1)
        self.pub_imu_yaw = rospy.Publisher('/' + self.imu_name +'/yaw', Float32, queue_size=1)

        self.sub = rospy.Subscriber(self.topic_name_angle, Imu, self.process_imu_message_angle, queue_size=1)
        self.sub = rospy.Subscriber(self.topic_name_reference, Imu, self.process_imu_message_reference, queue_size=1)

        rate = rospy.Rate(self.rate)

        while not rospy.is_shutdown():
            rate.sleep()