+ def get_tle_err(self):
+ err = struct.unpack(">B", i2c_read_reg(0x50, 0x94, 1))[0]
+
+ msg = DiagnosticArray()
+ msg.header.stamp = rospy.Time.now()
+ stat = DiagnosticStatus()
+ stat.name = "Motor: Error Status"
+ stat.level = DiagnosticStatus.OK if not err else DiagnosticStatus.ERROR
+ stat.message = "OK" if not err else "Error"
+
+ stat.values.append(KeyValue("Motor 1", str(bool(err & (1 << 0)))))
+ stat.values.append(KeyValue("Motor 2", str(bool(err & (1 << 1)))))
+ stat.values.append(KeyValue("Motor 3", str(bool(err & (1 << 2)))))
+ stat.values.append(KeyValue("Motor 4", str(bool(err & (1 << 3)))))
+
+ msg.status.append(stat)
+ self.pub_diag.publish(msg)
+
+