Ejemplo n.º 1
0
def integrate(dt, phase, orientation):
    """ Integrate motion to produce new orientation """
    phase += 2*pi*dt*Omegaf
    omega = Omega*cos(phase)
    delta = (np.eye(3), rotations.hat_map(dt*omega))
    orientation = SO3.multiply(orientation, SO3.expmap(delta))
    return phase, omega, orientation
Ejemplo n.º 2
0
def accelerometer(orientation):
    """ Produce gravity vector from orientation matrix """
    acceleration = SO3.multiply(SO3.inverse(orientation), Gravity)
    return acceleration
Ejemplo n.º 3
0
def run(pause=False):
    """ Run clock and integrator and emit values.
    """
    imupub = rospy.Publisher('~imu', Imu, queue_size=1)
    gyropub = rospy.Publisher('~gyro', Imu, queue_size=1)
    truepose = rospy.Publisher('~pose', PoseStamped, queue_size=1)
    clockpub = rospy.Publisher('/clock', Clock, queue_size=1)

    loop_hz = 125.
    dt = 1./loop_hz

    now = 0.0
    phase = np.zeros((3,))
    orientation = SO3.identity()
    seq = 0
    factor = 1.
    singlestep = False
    state = "PAUSE" if pause else "RUN"

    try:
        saveattrs = setiraw(0)
        while not rospy.is_shutdown():
            if not singlestep:
                time.sleep(dt/factor)
            if not pause:
                (imu, gyro, pose), (now, phase, orientation, seq) = step(dt, now, phase, orientation, seq)
                imupub.publish(imu)
                gyropub.publish(gyro)
                truepose.publish(pose)
                clockpub.publish(Clock(rospy.Time(now)))
                if singlestep:
                    pause = True
                    singlestep = False
            if factor == 1:
                print("\33[2K\r<%s %1.3f>"%(state,now), end="")
            else:
                print("\33[2K\r<RATE %1.3f, %s %1.3f>"%(factor, state, now), end="")
            sys.stdout.flush()
            ready = select.select([0], [], [], 0)
            if len(ready)>0:
                c = os.read(0, 1)
                if c==' ':
                    pause^=True
                    if pause:
                        state="PAUSE"
                    else:
                        state="RUN"
                if pause and c=='s':
                    singlestep = True
                    pause = False
                    state="STEP"
                if c=='+':
                    factor *= 2
                if c=='-':
                    factor /= 2
                if c=='q':
                    print("\33[2K\r<QUIT>", end="")
                    break
    finally:
        termios.tcsetattr(0, termios.TCSANOW, saveattrs)
        print("\33[2K\r<DONE>")