Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 import roslib; roslib.load_manifest('cob_hwboard')
00053 import rospy
00054 from serial import *
00055 from diagnostic_msgs.msg import DiagnosticArray
00056 from diagnostic_msgs.msg import DiagnosticStatus
00057 from diagnostic_msgs.msg import KeyValue
00058
00059 class HwBoard:
00060 def __init__(self):
00061
00062 rospy.init_node('hwboard')
00063
00064
00065 if not rospy.has_param("~devicestring"):
00066 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","devicestring")
00067 sys.exit()
00068 devicestring_param = rospy.get_param("~devicestring")
00069
00070 if not rospy.has_param("~head_sensor"):
00071 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","head_sensor")
00072 sys.exit()
00073 self.head_sensor_param = rospy.get_param("~head_sensor")
00074
00075 if not rospy.has_param("~eye_sensor"):
00076 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","eye_sensor")
00077 sys.exit()
00078 self.eye_sensor_param = rospy.get_param("~eye_sensor")
00079
00080 if not rospy.has_param("~torso_module_sensor"):
00081 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","torso_module_sensor")
00082 sys.exit()
00083 self.torso_module_sensor_param = rospy.get_param("~torso_module_sensor")
00084
00085 if not rospy.has_param("~torso_sensor"):
00086 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","torso_sensor")
00087 sys.exit()
00088 self.torso_sensor_param = rospy.get_param("~torso_sensor")
00089
00090 if not rospy.has_param("~pc_sensor"):
00091 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","pc_sensor")
00092 sys.exit()
00093 self.pc_sensor_param = rospy.get_param("~pc_sensor")
00094
00095 if not rospy.has_param("~engine_sensor"):
00096 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...","engine_sensor")
00097 sys.exit()
00098 self.engine_sensor_param = rospy.get_param("~engine_sensor")
00099
00100
00101 rospy.loginfo("trying to initializing serial connection")
00102 try:
00103 self.s = Serial(port=devicestring_param,baudrate=230400,bytesize=EIGHTBITS,parity=PARITY_NONE,stopbits=STOPBITS_ONE,timeout=3)
00104 except serial.serialutil.SerialException:
00105 rospy.logerr("Could not initialize serial connection on %s, aborting...",devicestring_param)
00106 sys.exit()
00107 rospy.loginfo("serial connection initialized successfully")
00108 self.s.open()
00109
00110
00111
00112 def reset(self):
00113
00114
00115 send_buff_array=[0xFF,0x0E,0x00,0x00,0x00]
00116 message= ""
00117 preamble_bytes = 4
00118 preamble_error = 1
00119 crc_error = 1
00120 retry = 0
00121
00122
00123 crc = 0x00
00124 for i in range(4):
00125 data = send_buff_array[i]
00126 for k in range(8):
00127 feedback_bit = (crc^data) & 0x80
00128 feedback_bit = (feedback_bit>>7) & 0xFF
00129
00130 if feedback_bit == 1:
00131 crc = (crc<<1) & 0xFF
00132 crc = crc^0x31
00133 else:
00134 crc = (crc<<1) & 0xFF
00135
00136 data = (data<<1) & 0xFF
00137 send_buff_array[4] = crc
00138
00139
00140 while (preamble_error == 1 or crc_error == 1) and retry < 8:
00141 message= ""
00142 for i in range(preamble_bytes):
00143 message += chr(0x55)
00144 for i in send_buff_array:
00145 message += chr(i)
00146 self.s.write(message)
00147
00148
00149
00150 read_buff_array = []
00151 buff = self.s.read(1)
00152 preamble_count = 0
00153 for i in buff:
00154 read_buff_array.append(ord(i))
00155
00156 if read_buff_array[0] == 0x55:
00157
00158
00159 while read_buff_array[0] == 0x55 and preamble_count < 10:
00160 read_buff_array = []
00161 buff = self.s.read(1)
00162 for i in buff:
00163 read_buff_array.append(ord(i))
00164 preamble_count = preamble_count + 1
00165
00166 buff = self.s.read(13)
00167
00168
00169
00170 if preamble_count > 6:
00171 preamble_error = 1
00172 preamble_bytes = preamble_bytes + 1
00173 retry = retry + 1
00174 if preamble_bytes == 7:
00175 preamble_bytes = 2
00176 elif preamble_count < 2:
00177 preamble_error = 1
00178 preamble_bytes = preamble_bytes + 1
00179 retry = retry + 1
00180 if preamble_bytes == 7:
00181 preamble_bytes = 2
00182 else:
00183
00184 preamble_error = 0
00185
00186
00187 for i in buff:
00188 read_buff_array.append(ord(i))
00189
00190
00191 crc = 0x00
00192 for i in range(14):
00193 data = read_buff_array[i]
00194 for k in range(8):
00195 feedback_bit = (crc^data) & 0x80
00196 feedback_bit = (feedback_bit>>7) & 0xFF
00197
00198 if feedback_bit == 1:
00199 crc = (crc<<1) & 0xFF
00200 crc = crc^0x31
00201 else:
00202 crc = (crc<<1) & 0xFF
00203
00204 data = (data<<1) & 0xFF
00205 if crc != 0:
00206 crc_error = 1
00207 preamble_bytes = preamble_bytes + 1
00208 retry = retry + 1
00209 if preamble_bytes == 7:
00210 preamble_bytes = 2
00211 else:
00212 crc_error = 0
00213
00214
00215 else:
00216 buff = s.read(14)
00217 preamble_error = 1
00218 preamble_bytes = preamble_bytes + 1
00219 retry = retry + 1
00220 if preamble_bytes == 7:
00221 preamble_bytes = 2
00222
00223
00224
00225 def hwboard(self):
00226
00227
00228 send_channel = 0
00229 read_channel = 0
00230 send_specifier = 0
00231 read_specifier = 0
00232 read_status = 0
00233 read_data = 0
00234 read_id = 0
00235 read_crc = 0
00236
00237
00238 pub = rospy.Publisher('diagnostics',DiagnosticArray)
00239
00240 while not rospy.is_shutdown():
00241
00242
00243 pub_message = DiagnosticArray()
00244
00245
00246 status_array = []
00247
00248
00249 error_while_reading = 0
00250
00251 for send_specifier in range(0,7,3):
00252
00253 if send_specifier == 0:
00254 count_range = range(6)
00255 elif send_specifier == 3:
00256 count_range = [0,1,2,3,6,7]
00257 else:
00258 count_range = [1,2,3,6,7]
00259
00260 for send_channel in count_range:
00261
00262
00263 send_buff_array = [send_channel,send_specifier,0x00,0x00,0x00]
00264 message = ""
00265 preamble_bytes = 4
00266 preamble_error = 1
00267 crc_error = 1
00268 retry = 0
00269
00270
00271 crc = 0x00
00272 for i in range(4):
00273 data = send_buff_array[i]
00274 for k in range(8):
00275 feedback_bit = (crc^data) & 0x80
00276 feedback_bit = (feedback_bit>>7) & 0xFF
00277
00278 if feedback_bit == 1:
00279 crc = (crc<<1) & 0xFF
00280 crc = crc^0x31
00281 else:
00282 crc = (crc<<1) & 0xFF
00283
00284 data = (data<<1) & 0xFF
00285 send_buff_array[4] = crc
00286
00287
00288 while (preamble_error == 1 or crc_error == 1) and retry < 8:
00289 message= ""
00290 for i in range(preamble_bytes):
00291 message += chr(0x55)
00292 for i in send_buff_array:
00293 message += chr(i)
00294 self.s.write(message)
00295
00296
00297
00298
00299
00300 read_buff_array = []
00301 buff = self.s.read(1)
00302 preamble_count = 0
00303 for i in buff:
00304 read_buff_array.append(ord(i))
00305
00306 if read_buff_array[0] == 0x55:
00307
00308
00309 while read_buff_array[0] == 0x55 and preamble_count < 10:
00310 read_buff_array = []
00311 buff = self.s.read(1)
00312 for i in buff:
00313 read_buff_array.append(ord(i))
00314 preamble_count = preamble_count + 1
00315
00316 buff = self.s.read(13)
00317
00318
00319
00320 if preamble_count > 6:
00321 preamble_error = 1
00322 preamble_bytes = preamble_bytes + 1
00323 retry = retry + 1
00324 if preamble_bytes == 7:
00325 preamble_bytes = 2
00326 elif preamble_count < 2:
00327 preamble_error = 1
00328 preamble_bytes = preamble_bytes + 1
00329 retry = retry + 1
00330 if preamble_bytes == 7:
00331 preamble_bytes = 2
00332 else:
00333
00334 preamble_error = 0
00335
00336
00337 for i in buff:
00338 read_buff_array.append(ord(i))
00339
00340
00341 crc = 0x00
00342 for i in range(14):
00343 data = read_buff_array[i]
00344 for k in range(8):
00345 feedback_bit = (crc^data) & 0x80
00346 feedback_bit = (feedback_bit>>7) & 0xFF
00347
00348 if feedback_bit == 1:
00349 crc = (crc<<1) & 0xFF
00350 crc = crc^0x31
00351 else:
00352 crc = (crc<<1) & 0xFF
00353
00354 data = (data<<1) & 0xFF
00355 if crc != 0:
00356 crc_error = 1
00357 preamble_bytes = preamble_bytes + 1
00358 retry = retry + 1
00359 if preamble_bytes == 7:
00360 preamble_bytes = 2
00361 else:
00362 crc_error = 0
00363
00364
00365 else:
00366 buff = s.read(14)
00367 preamble_error = 1
00368 preamble_bytes = preamble_bytes + 1
00369 retry = retry + 1
00370 if preamble_bytes == 7:
00371 preamble_bytes = 2
00372
00373
00374
00375
00376 read_channel = int(read_buff_array[0])
00377
00378
00379 read_specifier = int(read_buff_array[1])
00380
00381
00382 read_status = int(read_buff_array[2])
00383
00384
00385 read_data = 256 * int(read_buff_array[3])
00386 read_data = read_data + int(read_buff_array[4])
00387
00388
00389 read_id = read_buff_array[5]<<8
00390 read_id = (read_id | read_buff_array[6])<<8
00391 read_id = (read_id | read_buff_array[7])<<8
00392 read_id = read_id | read_buff_array[8]
00393
00394
00395 if read_channel == send_channel:
00396 if read_specifier == send_specifier:
00397 if read_status == 0 or read_status == 8:
00398 if send_specifier == 0:
00399 read_data = read_data / 10.0
00400 else:
00401 read_data = read_data / 1000.0
00402 erro_while_reading = 0
00403 else:
00404 read_data = 0
00405 error_while_reading = 1
00406 else:
00407 read_data = 0
00408 error_while_reading = 1
00409 else:
00410 read_data = 0
00411 error_while_reading = 1
00412
00413
00414
00415
00416
00417 status_object = DiagnosticStatus()
00418
00419
00420 key_value = KeyValue()
00421
00422
00423 if send_specifier == 0:
00424 if read_data == 85:
00425 level = 1
00426 status_object.message = "sensor damaged"
00427 elif read_data > 50:
00428 level = 2
00429 status_object.message = "temperature critical"
00430 elif read_data >40:
00431 level = 1
00432 status_object.message = "temperature high"
00433 elif read_data > 10:
00434 level = 0
00435 status_object.message = "temperature ok"
00436 elif read_data > -1:
00437 level = 1
00438 status_object.message = "temperature low"
00439 else:
00440 level = 2
00441 status_object.message = "temperature critical"
00442
00443
00444 if read_id == self.head_sensor_param:
00445 status_object.name = "Head Temperature"
00446 status_object.hardware_id = "hwboard_channel " + str(send_channel)
00447 elif read_id == self.eye_sensor_param:
00448 status_object.name = "Eye Camera Temperature"
00449 status_object.hardware_id = "hwboard_channel = " + str(send_channel)
00450 elif read_id == self.torso_module_sensor_param:
00451 status_object.name = "Torso Module Temperature"
00452 status_object.hardware_id = "hwboard_channel =" + str(send_channel)
00453 elif read_id == self.torso_sensor_param:
00454 status_object.name = "Torso Temperature"
00455 status_object.hardware_id = "hwboard_channel =" + str(send_channel)
00456 elif read_id == self.pc_sensor_param:
00457 status_object.name = "PC Temperature"
00458 status_object.hardware_id = "hwboard_channel =" + str(send_channel)
00459 elif read_id == self.engine_sensor_param:
00460 status_object.name = "Engine Temperature"
00461 status_object.hardware_id = "hwboard_channel = " + str(send_channel)
00462 else:
00463 level = 1
00464 status_object.message = "cannot map if from yaml file to temperature sensor"
00465
00466
00467 elif send_specifier == 3:
00468
00469 if send_channel == 0:
00470 if read_data > 58:
00471 level = 2
00472 status_object.message = "voltage critical"
00473 elif read_data > 56:
00474 level = 1
00475 status_object.message = "voltage high"
00476 elif read_data > 44:
00477 level = 0
00478 status_object.message = "voltage ok"
00479 elif read_data > 42:
00480 level = 1
00481 status_object.message = "voltage low"
00482 else:
00483 level = 2
00484 status_object.message = "voltage critical"
00485 else:
00486 if read_data > 27:
00487 level = 2
00488 status_object.message = "voltage critical"
00489 elif read_data > 25:
00490 level = 1
00491 status_object.message = "voltage_high"
00492 elif read_data > 23:
00493 level = 0
00494 status_object.message = "voltage ok"
00495 elif read_data > 19:
00496 level = 1
00497 status_object.message = "voltage low"
00498 else:
00499 level = 2
00500 status_object.message = "voltage critical"
00501
00502 if send_channel == 0:
00503 status_object.name = "Akku Voltage"
00504 status_object.hardware_id = "hwboard_channel = 0"
00505 elif send_channel == 1:
00506 status_object.name = "Torso Engine Voltage"
00507 status_object.hardware_id = "hwboard_channel = 1"
00508 elif send_channel == 2:
00509 status_object.name = "Torso Logic Voltage"
00510 status_object.hardware_id = "hwboard_channel = 2"
00511 elif send_channel == 3:
00512 status_object.name = "Tray Logic Voltage"
00513 status_object.hardware_id = "hwboard_channel = 3"
00514 elif send_channel == 6:
00515 status_object.name = "Arm Engine Voltage"
00516 status_object.hardware_id = "hwboard_channel = 6"
00517 elif send_channel == 7:
00518 status_object.name = "Tray Engine Voltage"
00519 status_object.hardware_id = "hwboard_channel = 7"
00520
00521
00522 else:
00523 if read_data > 15:
00524 level = 2
00525 status_object.message = "current critical"
00526 elif read_data > 10:
00527 level = 1
00528 status_object.message = "current high"
00529 elif read_data < 0:
00530 level = 2
00531 status_object.message = "current critical"
00532 else:
00533 level = 0
00534 status_object.message = "current ok"
00535
00536 if send_channel == 1:
00537 status_object.name = "Torso Engine Current"
00538 status_object.hardware_id = "hwboard_channel = 1"
00539 elif send_channel == 2:
00540 status_object.name = "Torso Logic Current"
00541 status_object.hardware_id = "hwboard_channel = 2"
00542 elif send_channel == 3:
00543 status_object.name = "Tray Logic Current"
00544 status_object.hardware_id = "hwboard_channel = 3"
00545 elif send_channel == 6:
00546 status_object.name = "Arm Engine Current"
00547 status_object.hardware_id = "hwboard_channel = 6"
00548 elif send_channel == 7:
00549 status_object.name = "Tray Engine Current"
00550 status_object.hardware_id = "hwboard_channel = 7"
00551
00552
00553 if error_while_reading == 1:
00554 level = 1
00555 status_object.message = "detected error while receiving answer from hardware"
00556
00557
00558
00559 status_object.level = level
00560 key_value.value = str(read_data)
00561 status_object.values.append(key_value)
00562 pub_message.status.append(status_object)
00563
00564
00565 pub.publish(pub_message)
00566 rospy.sleep(1.0)
00567
00568
00569
00570
00571
00572
00573
00574 if __name__ == '__main__':
00575 hwb = HwBoard()
00576 hwb.reset()
00577 hwb.hwboard()