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 send_status(system, importance, frequency, value):
status = drc.system_status_t()
status.utime = time.time() * 1e6
status.system = system
status.importance = importance
status.frequency = 0
status.value = value
lcm.LCM().publish('SYSTEM_STATUS', status.encode())
def send_status(system, importance, frequency, value):
status = drc.system_status_t()
status.utime = time.time() * 1e6
status.system = system
status.importance = importance
status.frequency = 0
status.value = value
lcm.LCM().publish('SYSTEM_STATUS', status.encode())
def publishSystemStatus(text):
msg = lcmdrc.system_status_t()
msg.utime = getUtime()
msg.system = 5
msg.importance = 0
msg.frequency = 0
msg.value = text
lcmWrapper.publish('SYSTEM_STATUS', msg)
def publishSystemStatus(side, lcm, status):
global connectPublished
global activePublished
if connectPublished and activePublished:
return
msg = drc.system_status_t()
msg.utime = (time() * 1000000)
msg.system = 4 #provided as the system level for grippers
msg.importance = 0
msg.frequency = 0
if connectPublished:
if status and status.activated == 1:
msg.value = side.upper() + " ROBOTIQ HAND ACTIVE: Receiving status and active"
lcm.publish("SYSTEM_STATUS", msg.encode())
activePublished = True
else:
if status:
msg.value = side.upper() + " ROBOTIQ HAND ALIVE: Receiving status messages"
lcm.publish("SYSTEM_STATUS", msg.encode())
connectPublished = True
