def onUseNewMapButton(self):
'''
Send a trigger to the GPF to use the newly created map
'''
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('STATE_EST_USE_NEW_MAP', msg)
def handle(self, channel, data):
msg = drc.controller_status_t.decode(data)
t = msg.utime / 1e6
if self.last_t is None or t - self.last_t > 1 or t < 0.001:
self.last_t = t
self.vdot_errror_count = 0
self.vdot_history = deque()
else:
if len(self.vdot_history) >= self.opts.memory:
self.vdot_errror_count -= int(self.vdot_history.popleft())
err = msg.Vdot > self.opts.vdot_threshold
# print msg.Vdot
if err:
print "Positive Vdot ({:.5f}) at time: {:.3f}".format(msg.Vdot, t)
self.vdot_history.append(err)
self.vdot_errror_count += int(err)
if self.vdot_errror_count > self.opts.count_threshold:
if time.time() - self.last_publish_time > self.opts.retransmit_delay:
print "WARNING: Fall detected at time: {:.3f}".format(t)
print "Bracing NOW"
brace_msg = drc.utime_t()
brace_msg.utime = 0
m.publish("BRACE_FOR_FALL", brace_msg.encode())
status_msg = drc.system_status_t()
status_msg.utime = msg.utime
status_msg.system = 8
status_msg.importance = 0
status_msg.frequency = 0
status_msg.value = 'Possible fall detected. Bracing now!'
m.publish('SYSTEM_STATUS', status_msg.encode())
self.last_publish_time = time.time()
self.last_t = t
def onStartNewMapButton(self):
'''
Send a trigger to the Create Octomap process to start a new map
'''
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('STATE_EST_START_NEW_MAP', msg)
def sendCalibrateEncodersCommand(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('CALIBRATE_ARM_ENCODERS', msg)
def onDisableLaserButton(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('STATE_EST_LASER_DISABLE', msg)
def onRestartNavButton(self):
ready_init = lcmdrc.utime_t()
ready_init.utime = getUtime()
lcmUtils.publish('STATE_EST_RESTART', ready_init)
def sendReadyMessage(self):
ready_init = lcmdrc.utime_t()
ready_init.utime = getUtime()
lcmUtils.publish('STATE_EST_READY', ready_init)
sleep(1) # sleep needed to give SE time to restart
def onRestartNavButton(self):
ready_init = lcmdrc.utime_t()
ready_init.utime = getUtime()
lcmUtils.publish('STATE_EST_RESTART', ready_init)
def disableArmEncoders():
msg = lcmdrc.utime_t()
msg.utime = -1
lcmUtils.publish('ENABLE_ENCODERS', msg)
def sendHaltSimulationDrakeRequest(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('HALT_DRAKE_SIMULATION', msg)
def onEnableLaserButton(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish("STATE_EST_LASER_ENABLE", msg)
def sendAutonmousTestDone(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('AUTONOMOUS_TEST_WALKING_DONE', msg)
def sendMITStandCommand(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('START_MIT_STAND', msg)
def disableArmEncoders():
msg = lcmdrc.utime_t()
msg.utime = -1
lcmUtils.publish('ENABLE_ENCODERS', msg)
def onDisableLaserButton(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('STATE_EST_LASER_DISABLE', msg)
def sendHaltSimulationDrakeRequest(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('HALT_DRAKE_SIMULATION', msg)
def sendCalibrateEncodersCommand(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('CALIBRATE_ARM_ENCODERS', msg)
def sendMITStandCommand(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('START_MIT_STAND', msg)
def sendAutonmousTestDone(self):
msg = lcmdrc.utime_t()
msg.utime = getUtime()
lcmUtils.publish('AUTONOMOUS_TEST_WALKING_DONE', msg)
def sendReadyMessage(self):
ready_init = lcmdrc.utime_t()
ready_init.utime = getUtime()
lcmUtils.publish('STATE_EST_READY', ready_init)
time.sleep(1) # sleep needed to give SE time to restart
