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 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 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