imu_to_angles.py 文件源码

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

        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0

        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, Imu, self.process_imu_message, queue_size=1)

        rate = rospy.Rate(self.rate)

        while not rospy.is_shutdown():

            rate.sleep()