def _get_status(self):
"""
Omits some errors that aren't really errors. The overload error is treated as a warning
"""
status = self.diagnostics_msg.status[0].level
if status == DiagnosticStatus().ERROR and self._receiving():
# Check that the FT sensor reports new values
w1 = from_wrench(self.raw_queue[0].wrench)
w2 = from_wrench(self.raw_queue[self.queue_len-1].wrench)
status = DiagnosticStatus().ERROR if np.allclose(w1, w2) else DiagnosticStatus().OK
return status
def _get_status(self):
"""
Omits some errors that aren't really errors. The overload error is treated as a warning
"""
status = self.diagnostics_msg.status[0].level
if status == DiagnosticStatus().ERROR and self._receiving():
# Check that the FT sensor reports new values
w1 = from_wrench(self.raw_queue[0].wrench)
w2 = from_wrench(self.raw_queue[self.queue_len - 1].wrench)
status = DiagnosticStatus().ERROR if np.allclose(
w1, w2) else DiagnosticStatus().OK
return status
def get_status(self):
if not self.initialized:
return DiagnosticStatus.ERROR
if not self.is_msg_recent(lambda : self.diagnostics_msg, delta_time=2.0):
status = DiagnosticStatus.ERROR
else:
status = self.diagnostics_msg.status[0].level
if status == DiagnosticStatus.ERROR:
# Check that the FT sensor reports new values
w1 = from_wrench(self.raw_queue[0].wrench)
w2 = from_wrench(self.raw_queue[self.queue_len-1].wrench)
status = DiagnosticStatus.ERROR if np.allclose(w1, w2) else DiagnosticStatus().OK
return status
def get_status(self):
if not self.initialized:
return DiagnosticStatus.ERROR
if not self.is_msg_recent(lambda: self.diagnostics_msg,
delta_time=2.0):
status = DiagnosticStatus.ERROR
else:
status = self.diagnostics_msg.status[0].level
if status == DiagnosticStatus.ERROR:
# Check that the FT sensor reports new values
w1 = from_wrench(self.raw_queue[0].wrench)
w2 = from_wrench(self.raw_queue[self.queue_len - 1].wrench)
status = DiagnosticStatus.ERROR if np.allclose(
w1, w2) else DiagnosticStatus().OK
return status
def get_compensated(self):
status = DiagnosticStatus().ERROR
value = None
timestamp = None
if self.is_compensated_alive():
status = self._get_status()
timestamp = self.compensated_msg.header.stamp.to_sec()
value = from_wrench(self.compensated_msg.wrench)
return status, timestamp, value
def get_raw(self):
status = DiagnosticStatus().ERROR
value = None
timestamp = None
if self.is_raw_alive():
status = self._get_status()
timestamp = self.raw_msg.header.stamp.to_sec()
value = from_wrench(self.raw_msg.wrench)
return status, timestamp, value
def get_raw_wrench(self):
value = None
if self.is_raw_alive():
value = from_wrench(self.raw_msg.wrench)
return value
def get_compensated_wrench(self):
value = None
if self.is_compensated_alive():
value = from_wrench(self.compensated_msg.wrench)
return value
def get_raw_wrench(self):
value = None
if self.is_raw_alive():
value = from_wrench(self.raw_msg.wrench)
return value
def get_compensated_wrench(self):
value = None
if self.is_compensated_alive():
value = from_wrench(self.compensated_msg.wrench)
return value
