def check_vitals(self, stat):
try:
status_front = roboclaw.ReadError(self.address_front)[1]
status_back = roboclaw.ReadError(self.address_back)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
state, message = self.ERRORS[status_front]
stat.summary(state, message)
state, message = self.ERRORS[status_back]
stat.summary(state, message)
try:
stat.add("front Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address_front)[1] / 10))
stat.add("front Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address_front)[1] / 10))
stat.add("front Temp1 C:", float(roboclaw.ReadTemp(self.address_front)[1] / 10))
stat.add("front Temp2 C:", float(roboclaw.ReadTemp2(self.address_front)[1] / 10))
stat.add("back Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address_back)[1] / 10))
stat.add("back Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address_back)[1] / 10))
stat.add("back Temp1 C:", float(roboclaw.ReadTemp(self.address_back)[1] / 10))
stat.add("back Temp2 C:", float(roboclaw.ReadTemp2(self.address_back)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
def check_vitals(self, stat):
try:
mutex.acquire()
status = roboclaw.ReadError(self.address)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
mutex.release()
return
mutex.release()
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
mutex.acquire()
stat.add(
"Main Batt V:",
float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10))
stat.add(
"Logic Batt V:",
float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10))
stat.add("Temp1 C:",
float(roboclaw.ReadTemp(self.address)[1] / 10))
stat.add("Temp2 C:",
float(roboclaw.ReadTemp2(self.address)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
finally:
mutex.release()
return stat
def check_vitals_front_motors(self, stat):
try:
status = roboclaw.ReadError(self.addressFrontMotors)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
stat.add(
"Main Batt V:",
float(
roboclaw.ReadMainBatteryVoltage(self.addressFrontMotors)[1]
/ 10))
stat.add(
"Logic Batt V:",
float(
roboclaw.ReadLogicBatteryVoltage(
self.addressFrontMotors)[1] / 10))
stat.add("Temp1 C:",
float(roboclaw.ReadTemp(self.addressFrontMotors)[1] / 10))
stat.add(
"Temp2 C:",
float(roboclaw.ReadTemp2(self.addressFrontMotors)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
def check_vitals(self, stat):
self.ERRORS = {
0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage high"),
0x0080:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
}
try:
status = roboclaw.ReadError(address)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
stat.add("Main Batt V:",
float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10))
stat.add("Logic Batt V:",
float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10))
stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.address)[1] / 10))
stat.add("Temp2 C:", float(roboclaw.ReadTemp2(self.address)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
def check_vitals(self, stat):
rospy.logdebug("checking vitals")
for addr in self.addresses:
try:
status = roboclaw.ReadError(addr)[1]
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
continue
state, message = self.ERRORS[status]
stat.summary(state, message)
try:
stat.add("%d Main Batt V:" % addr,
float(roboclaw.ReadMainBatteryVoltage(addr)[1] / 10))
stat.add("%d Logic Batt V:" % addr,
float(roboclaw.ReadLogicBatteryVoltage(addr)[1] / 10))
stat.add("%d Temp1 C:" % addr,
float(roboclaw.ReadTemp(addr)[1] / 10))
stat.add("%d Temp2 C:" % addr,
float(roboclaw.ReadTemp2(addr)[1] / 10))
except OSError as e:
rospy.logwarn("Diagnostics OSError: %d", e.errno)
rospy.logdebug(e)
return stat
