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
def accelerometer(orientation):
""" Produce gravity vector from orientation matrix """
acceleration = SO3.multiply(SO3.inverse(orientation), Gravity)
return acceleration
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>")