cob_hwboard.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #################################################################
00003 # 
00004 # Copyright (c) 2012
00005 # Fraunhofer Institute for Manufacturing Engineering
00006 # and Automation (IPA)
00007 #
00008 #################################################################
00009 #
00010 # Project name: care-o-bot
00011 # ROS stack name: cob_driver
00012 # ROS package name: cob_hwboard
00013 #
00014 # Author: Eduard Herkel, email: eduard.herkel@ipa.fraunhofer.de
00015 # Supervised by: Eduard Herkel, email: eduard.herkel@ipa.fraunhofer.de
00016 #
00017 # Date of creation: October 2012
00018 #
00019 # ToDo
00020 #
00021 #################################################################
00022 #
00023 # Redistribution and use in source and binary forms, with or without
00024 # modification, are permitted provided that the following conditions are met:
00025 #
00026 # - Redistributions of source code must retain the above copyright
00027 # notice, this list of conditions and the following disclaimer. \n
00028 # - Redistributions in binary form must reproduce the above copyright
00029 # notice, this list of conditions and the following disclaimer in the
00030 # documentation and/or other materials provided with the distribution. \n
00031 # - Neither the name of the Fraunhofer Institute for Manufacturing
00032 # Engineering and Automation (IPA) nor the names of its
00033 # contributors may be used to endorse or promote products derived from
00034 # this software without specific prior written permission. \n
00035 #
00036 # This program is free software: you can redistribute it and/or modify
00037 # it under the terms of the GNU Lesser General Public License LGPL as
00038 # published by the Free Software Foundation, either version 3 of the
00039 # License, or (at your option) any later version.
00040 #
00041 # This program is distributed in the hope that it will be useful,
00042 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00043 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00044 # GNU Lesser General Public License LGPL for more details.
00045 #
00046 # You should have received a copy of the GNU Lesser General Public
00047 # License LGPL along with this program.
00048 # If not, see <http://www.gnu.org/licenses/>.
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                 # get parameters from parameter server
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                 # open serial connection
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                 # initialize message and local variables
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                 # calculate crc
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                 # send message
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                 #receive message
00149                         # check for first preamble byte of reveiced message
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                                 # check for following preamble bytes
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                                 # check preamble length
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                                         # preamble ok. evaluate message
00184                                         preamble_error = 0
00185 
00186                                         # get remaining message
00187                                         for i in buff:
00188                                                 read_buff_array.append(ord(i))
00189 
00190                                         #check crc
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                         # no preamble detected
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                 # initialize local variables
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                 # init ros-node
00238                 pub = rospy.Publisher('diagnostics',DiagnosticArray)
00239                 
00240                 while not rospy.is_shutdown():
00241 
00242                         # init publisher message
00243                         pub_message = DiagnosticArray() 
00244 
00245                         # init array for storing data
00246                         status_array = []
00247 
00248                         # init local variable for error detection
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                                         # init message and local variables
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                                         # calculate crc
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                                         # send message
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                                         # receive message
00299                                                 # check for first preamble byte of reveiced message
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                                                         # check for following preamble bytes
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                                                         # check preamble length
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                                                                 # preamble ok. evaluate message
00334                                                                 preamble_error = 0
00335 
00336                                                                 # get remaining message
00337                                                                 for i in buff:
00338                                                                         read_buff_array.append(ord(i))
00339 
00340                                                                 #check crc
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                                                 # no preamble detected
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                                         # get channel byte
00376                                         read_channel = int(read_buff_array[0])
00377                 
00378                                         # get specifier byte
00379                                         read_specifier = int(read_buff_array[1])
00380 
00381                                         # get status byte
00382                                         read_status = int(read_buff_array[2])
00383 
00384                                         # get data bytes
00385                                         read_data = 256 * int(read_buff_array[3])
00386                                         read_data = read_data + int(read_buff_array[4])
00387 
00388                                         # get id bytes
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                                         # evaluate recieved message
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                                         #prepare status object for publishing
00415 
00416                                         # init sensor object
00417                                         status_object = DiagnosticStatus()
00418 
00419                                         # init local variable for data
00420                                         key_value = KeyValue()
00421 
00422                                         # set values for temperature parameters
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                                                 # mapping for temperature sensors
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                                         # set values for voltage parameters
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                                         # set values for current parameters
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                                         # evaluate error detection
00553                                         if error_while_reading == 1:
00554                                                 level = 1
00555                                                 status_object.message = "detected error while receiving answer from hardware"
00556 
00557                                         # append status object to publishing message
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                         # publish message
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()


cob_hwboard
Author(s): Eduard Herkel
autogenerated on Tue Mar 3 2015 15:11:51