add_orientation_offset.py 文件源码

def __init__(self):

        if rospy.has_param('~orientation_offset'):
            # Orientation offset as quaterion q = [x,y,z,w].
            self.orientation_offset = rospy.get_param('~orientation_offset')
        else:
            yaw_offset_deg = rospy.get_param('~yaw_offset_deg', 0.0)
            self.orientation_offset = tf.quaternion_from_euler(0.0, 0.0, math.radians(yaw_offset_deg))

        rospy.Subscriber(rospy.get_name() + "/imu_in", Imu, self.imu_callback)

        self.pub_imu_out = rospy.Publisher(rospy.get_name() + '/imu_out',
                                           Imu, queue_size=10)

        rospy.spin()