reference_imu_to_angle.py 文件源码

def publish_angles(self):
        if  self.enable_reference and self.enable_angle:
            self.pub_imu_roll_msg = Float32()
            self.pub_imu_roll_msg.data = self.roll_frame - self.roll_steer
            self.pub_imu_roll.publish(self.pub_imu_roll_msg)

            self.pub_imu_pitch_msg = Float32()
            self.pub_imu_pitch_msg.data = self.pitch_frame - self.pitch_steer
            self.pub_imu_pitch.publish(self.pub_imu_pitch_msg)

            self.pub_imu_yaw_msg = Float32()
            self.pub_imu_yaw_msg.data = self.yaw_frame - self.yaw_steer
            self.pub_imu_yaw.publish(self.pub_imu_yaw_msg)


# Main function.