Source Code for Module eddiebot_node.diagnostics

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2012, Tang Tiong Yew 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of the Willow Garage nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32   
 33  #Tang Tiong Yew <tang.tiong.yew@gmail.com> 
 34   
 35  import rospy 
 36   
 37  from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 
 38   
 39 -class EddiebotDiagnostics(): 
 40 -    def __init__(self): 
 41          self.charging_state =  {0:"Not Charging", 
 42                                  1:"Reconditioning Charging", 
 43                                  2:"Full Charging", 
 44                                  3:"Trickle Charging", 
 45                                  4:"Waiting", 
 46                                  5:"Charging Fault Condition"} 
 47          self.charging_source = {0:"None", 
 48                                  1:"Internal Charger", 
 49                                  2:"Base Dock"} 
 50          self.digital_outputs = {0:"OFF", 
 51                                  1:"ON"} 
 52          self.oi_mode = {1:"Passive", 
 53                          2:"Safe", 
 54                          3:"Full"} 
 55          self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray) 
 56          self.last_diagnostics_time = rospy.get_rostime()         
 57   
 58 -    def node_status(self, msg, status): 
 59          curr_time = rospy.Time.now() 
 60          diag = DiagnosticArray() 
 61          diag.header.stamp = curr_time 
 62          stat = DiagnosticStatus() 
 63          if status == "error": 
 64              stat = DiagnosticStatus(name="EddieBot Node", level=DiagnosticStatus.ERROR, message=msg) 
 65          if status == "warn": 
 66              stat = DiagnosticStatus(name="EddieBot Node", level=DiagnosticStatus.WARN, message=msg) 
 67          diag.status.append(stat) 
 68          self.diag_pub.publish(diag) 
 69   
 70   
 71 -    def publish(self, sensor_state, gyro): 
 72          curr_time = sensor_state.header.stamp 
 73          # limit to 5hz 
 74          if (curr_time - self.last_diagnostics_time).to_sec() < 0.2: 
 75              return 
 76          self.last_diagnostics_time = curr_time 
 77   
 78          diag = DiagnosticArray() 
 79          diag.header.stamp = curr_time 
 80          stat = DiagnosticStatus() 
 81   
 82          #node status 
 83          stat = DiagnosticStatus(name="EddieBot Node", level=DiagnosticStatus.OK, message="RUNNING") 
 84          diag.status.append(stat) 
 85   
 86          #mode info 
 87          stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK) 
 88          try: 
 89              stat.message = self.oi_mode[sensor_state.oi_mode] 
 90          except KeyError as ex: 
 91              stat.level=DiagnosticStatus.ERROR 
 92              stat.message = "Invalid OI Mode Reported %s"%ex 
 93              rospy.logwarn(stat.message) 
 94          diag.status.append(stat) 
 95           
 96          #battery info 
 97          stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK") 
 98          values = stat.values 
 99          if sensor_state.charging_state == 5: 
100              stat.level = DiagnosticStatus.ERROR 
101              stat.message = "Charging Fault Condition" 
102              values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state])) 
103          values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)), 
104                         KeyValue("Current (A)", str(sensor_state.current/1000.0)), 
105                         KeyValue("Temperature (C)",str(sensor_state.temperature)), 
106                         KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)), 
107                         KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))]) 
108          diag.status.append(stat) 
109           
110          #charging source 
111          stat = DiagnosticStatus(name="Charging Sources", level=DiagnosticStatus.OK) 
112          try: 
113              stat.message = self.charging_source[sensor_state.charging_sources_available] 
114          except KeyError as ex: 
115              stat.level=DiagnosticStatus.ERROR 
116              stat.message = "Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available) 
117              rospy.logwarn(stat.message) 
118          diag.status.append(stat) 
119          #cliff sensors 
120          stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK") 
121          if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right: 
122              stat.level = DiagnosticStatus.WARN 
123              if (sensor_state.current/1000.0)>0: 
124                  stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff." 
125                  stat.level = DiagnosticStatus.OK # We're OK here 
126              else: 
127                  stat.message = "Near Cliff" 
128          stat.values = [KeyValue("Left", str(sensor_state.cliff_left)), 
129                         KeyValue("Left Signal", str(sensor_state.cliff_left_signal)), 
130                         KeyValue("Front Left", str(sensor_state.cliff_front_left)), 
131                         KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)), 
132                         KeyValue("Front Right", str(sensor_state.cliff_right)), 
133                         KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)), 
134                         KeyValue("Right", str(sensor_state.cliff_front_right)), 
135                         KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))] 
136          diag.status.append(stat) 
137          #Wall sensors 
138          stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK") 
139          #wall always seems to be false???  
140          if sensor_state.wall: 
141              stat.level = DiagnosticStatus.ERROR 
142              stat.message = "Near Wall" 
143          stat.values = [KeyValue("Wall", str(sensor_state.wall)), 
144                         KeyValue("Wall Signal", str(sensor_state.wall_signal)), 
145                         KeyValue("Virtual Wall", str(sensor_state.virtual_wall))] 
146          diag.status.append(stat) 
147          #Gyro 
148          stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK") 
149          if gyro is None: 
150              stat.level = DiagnosticStatus.WARN 
151              stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the eddiebot_node."    
152          elif gyro.cal_offset < 100: 
153              stat.level = DiagnosticStatus.ERROR 
154              stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/eddiebot-bad-gyro-calibration-also." 
155          elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0: 
156              stat.level = DiagnosticStatus.ERROR 
157              stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation." 
158   
159          if gyro is not None:     
160              stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)), 
161                             KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)), 
162                             KeyValue("Calibration Offset", str(gyro.cal_offset)), 
163                             KeyValue("Calibration Buffer", str(gyro.cal_buffer))] 
164          diag.status.append(stat) 
165          #Digital IO 
166          stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK") 
167          out_byte = sensor_state.user_digital_outputs 
168          stat.values = [KeyValue("Raw Byte", str(out_byte)), 
169                         KeyValue("Digital Out 2", self.digital_outputs[out_byte%2]), 
170                         KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]), 
171                         KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>2)%2])] 
172          diag.status.append(stat) 
173          #publish 
174          self.diag_pub.publish(diag) 
175