Exemple #1
0
                                              0.0, 0.0001, 0.0,
                                              0.0, 0.0, 100.0]
    #
    # linear acceleration not available
    #
    imuMessage.linear_acceleration_covariance = [-1.0, 0.0, 0.0,
                                                 0.0, 0.0, 0.0,
                                                 0.0, 0.0, 0.0]

    yawOffset = None
    previousTimestamp = rospy.get_time()
    previousYaw = None

    while not rospy.is_shutdown():
        currentTimestamp = rospy.get_time()
        roll, pitch, yaw, exception = imu.getOrientation()
        if exception:
            rospy.logwarn("getOrientation() exception: %s" % (exception))
            continue

        deltaTime = currentTimestamp - previousTimestamp

#        roll = roll / 180 * pi
#        pitch = pitch / 180 * pi
        try:
            yaw = -1 * yaw / 180 * pi
        except:
            rospy.logwarn('Exception converting raw yaw %d to radians' % (yaw))
            continue

        if not yawOffset: