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 twist_callback(msg):
global pub
global last_time
last_time = rospy.get_rostime()
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
TR = MIX_ROTATION
v_l = msg.linear.x - TR * msg.angular.z
v_r = msg.linear.x + TR * msg.angular.z
dutyleft = int(v_l * MAX_DUTY_CYCLE * 32768)
dutyright = int(v_r * MAX_DUTY_CYCLE * 32768)
dutyleft = limit(dutyleft, 32767)
dutyright = limit(dutyright, 32767)
rc.DutyM1M2(MTR_ADDRESS, dutyleft, dutyright)
rospy.loginfo("[%d , %d]" % (dutyleft, dutyright))
dummy, c1, c2 = rc.ReadCurrents(MTR_ADDRESS)
status = rc.ReadError(MTR_ADDRESS)[1]
bvoltage = rc.ReadMainBatteryVoltage(MTR_ADDRESS)[1] / 10.0
diagstr = "BattVoltage %f, Current[%f,%f], Status 0x%x" % (
bvoltage, c1 / 100.0, c2 / 100.0, status)
pub.publish(diagstr)
def debug_robot():
rc.Open('/dev/ttyUSB0',115200)
while True:
status = rc.ReadError(MTR_ADDRESS)[1]
dummy,c1,c2 = rc.ReadCurrents(MTR_ADDRESS)
bvoltage =rc.ReadMainBatteryVoltage(MTR_ADDRESS)[1] / 10.0
diagstr = "BattVoltage %f, Current[%f,%f], Status 0x%x" % (bvoltage, c1/100.0, c2/100.0, status)
print diagstr
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
