Python msg.DiagnosticStatus类代码示例

 def info_status(self):
     stat = DiagnosticStatus(name="Info_Platform", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("", str(self.robot.get_status()[0])),
         KeyValue("", str(self.robot.get_status()[1])),
         KeyValue("", str(self.robot.get_status()[2]))]
     return stat

    def spin(self):
        r = rospy.Rate(self._diagnostic_freq)
        while not rospy.is_shutdown():
            diag = DiagnosticArray()
            diag.header.stamp = rospy.get_rostime()
            for mon in self._monitors:
                d = DiagnosticStatus()
                d.name=mon.get_name()
                d.hardware_id="PC0"
                d.message=mon.get_field_value("status")
                d.level=mon.get_field_value("status_level")
                    

                for key in mon.get_fields():
                    p = KeyValue()
                    p.key = key
                    p.value = str(mon.get_field_value(key))
                    d.values.append(p)
                diag.status.append(d)
            self._diagnostic_pub.publish(diag)
            
            r.sleep()
            
        self._cpu_temp_mon.stop()
        self._cpu_usage_mon.stop()
        self._wlan_mon.stop()
        self._bandwidth_mon.stop()

 def fill_diags(name, summary, hid, diags):
     ds = DiagnosticStatus()
     ds.values = [KeyValue(k, str(v)) for (k, v) in diags]
     ds.hardware_id = hid
     ds.name = rospy.get_caller_id().lstrip('/') + ": " + name
     ds.message = summary
     return ds

 def diagnostics_processor(self):
     diag_msg = DiagnosticArray()
     
     rate = rospy.Rate(self.diagnostics_rate)
     while not rospy.is_shutdown():
         diag_msg.status = []
         diag_msg.header.stamp = rospy.Time.now()
         
         for controller in self.controllers.values():
             try:
                 joint_state = controller.joint_state
                 temps = joint_state.motor_temps
                 max_temp = max(temps)
                 
                 status = DiagnosticStatus()
                 status.name = 'Joint Controller (%s)' % controller.joint_name
                 status.hardware_id = 'Robotis Dynamixel %s on port %s' % (str(joint_state.motor_ids), controller.port_namespace)
                 status.values.append(KeyValue('Goal', str(joint_state.goal_pos)))
                 status.values.append(KeyValue('Position', str(joint_state.current_pos)))
                 status.values.append(KeyValue('Error', str(joint_state.error)))
                 status.values.append(KeyValue('Velocity', str(joint_state.velocity)))
                 status.values.append(KeyValue('Load', str(joint_state.load)))
                 status.values.append(KeyValue('Moving', str(joint_state.is_moving)))
                 status.values.append(KeyValue('Temperature', str(max_temp)))
                 status.level = DiagnosticStatus.OK
                 status.message = 'OK'
                     
                 diag_msg.status.append(status)
             except:
                 pass
                 
         self.diagnostics_pub.publish(diag_msg)
         rate.sleep()

 def speed_status(self):
     stat = DiagnosticStatus(name="Speed", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("Linear speed (Vx)", str(self.robot.get_movesteer(None)[0])),
         KeyValue("Angular speed (Vth)", str(self.robot.get_movesteer(None)[2]))]
     if self.robot.get_movesteer(None)[0] > self.speedLimit:
         stat.level = DiagnosticStatus.WARN
         stat.message = "speed is too high"
     return stat

 def connection_status(self):
     stat = DiagnosticStatus(name="Connection", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("Baudrate", str(self.robot.get_connection_info()["baudrate"])),
         KeyValue("Comport", str(self.robot.get_connection_info()["comport"]))]
     if self.robot.is_connected() == False:
         stat.message = "disconnected"
         stat.level = DiagnosticStatus.ERROR
     return stat

def send_diags():
    # See diagnostics with: rosrun rqt_runtime_monitor rqt_runtime_monitor
    msg = DiagnosticArray()
    msg.status = []
    msg.header.stamp = rospy.Time.now()
    
    for gripper in grippers:
        for servo in gripper.servos:
            status = DiagnosticStatus()
            status.name = "Gripper '%s' servo %d"%(gripper.name, servo.servo_id)
            status.hardware_id = '%s'%servo.servo_id
            temperature = servo.read_temperature()
            status.values.append(KeyValue('Temperature', str(temperature)))
            status.values.append(KeyValue('Voltage', str(servo.read_voltage())))
    
            if temperature >= 70:
                status.level = DiagnosticStatus.ERROR
                status.message = 'OVERHEATING'
            elif temperature >= 65:
                status.level = DiagnosticStatus.WARN
                status.message = 'HOT'
            else:
                status.level = DiagnosticStatus.OK
                status.message = 'OK'
        
            msg.status.append(status)
    
    diagnostics_pub.publish(msg)

def _camera_diag(level = 0):
    array = DiagnosticArray()
    stat = DiagnosticStatus()
    stat.name = 'wge100: Driver Status'
    stat.level = level
    stat.message = 'OK'

    array.header.stamp = rospy.get_rostime()
    array.status.append(stat)
    
    return array

def test_DiagnosticStatus():
    '''
    PUBLISHER METHODE: DiagnosticStatus
    '''
    pub_DiagnosticStatus = rospy.Publisher('diagnostic', DiagnosticStatus, queue_size=10)

    message = DiagnosticStatus()
    message.name = "ROB"
    message.message = "RUNNING"

    print "DiagnosticStatus: " + message.name + " , " + message.message
    pub_DiagnosticStatus.publish(message)

    def publish_diagnostics(self, time):
        diag = DiagnosticArray()
        diag.header.stamp = time
        
        stat = DiagnosticStatus(name="Battery Status", level=DiagnosticStatus.OK, message=self.power_state_msg)
        if self.power_state == 3:
            stat.level=DiagnosticStatus.WARN
        if self.power_state == 4:
            stat.level=DiagnosticStatus.ERROR
        diag.status.append(stat)

        self.diag_pub.publish(diag)

 def battery_status(self):
     stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("Voltage avg(Vin)", str(self.robot.get_avg_voltageIn())),
         KeyValue("Voltage max(Vin)", str(self.robot.get_max_voltageIn())),
         KeyValue("Voltage min(Vin)", str(self.robot.get_min_voltageIn())),
         KeyValue("Current avg(A)", str(self.robot.get_avg_current())),
         KeyValue("Current max(A)", str(self.robot.get_avg_current())),
         KeyValue("Current min(A)", str(self.robot.get_avg_current())),
         KeyValue("Voltage avg(V5V)", str(self.robot.get_avg_5voltage())),
         KeyValue("Voltage max(V5V)", str(self.robot.get_max_5voltage())),
         KeyValue("Voltage min(V5V)", str(self.robot.get_min_5voltage())),
         KeyValue("Voltage avg(V3.3)", str(self.robot.get_avg_3voltage())),
         KeyValue("Voltage max(V3.3)", str(self.robot.get_max_3voltage())),
         KeyValue("Voltage min(V3.3)", str(self.robot.get_min_3voltage()))]
     if self.robot.get_status().VoltageLow == True:
         stat.level = DiagnosticStatus.WARN
         stat.message = "Voltage too low"
     if self.robot.get_status().CurrentError == True:
         stat.level = DiagnosticStatus.WARN
         stat.message = "Current error"
     if self.robot.get_status().Voltage3v3Low == True:
         stat.level = DiagnosticStatus.WARN
         stat.message = "Voltage3.3 too low"
     return stat

    def publish_diagnostics(self, time):
        diag = DiagnosticArray()
        diag.header.stamp = time        
        
        for sphero_name in self.sphero_dict:
            sphero = self.sphero_dict[sphero_name]
            stat = DiagnosticStatus(name=sphero_name, level=DiagnosticStatus.OK, message=sphero.power_state_msg)
            #stat.message="Battery Status"
            if sphero.power_state == 3:
                stat.level=DiagnosticStatus.WARN
            if sphero.power_state == 4:
                stat.level=DiagnosticStatus.ERROR
            diag.status.append(stat)

        self.diag_pub.publish(diag)

 def transform(self, msg):
   statuses = msg.status
   for status in statuses:
       if not status.name.startswith('/'):
           status.name = '/' + status.name
 
   def top_only(status): return status.name.count("/") < 2    
   filtered_statuses = filter(top_only, statuses)
   levels = [status.level for status in filtered_statuses]
   
   new_message = DiagnosticStatus()
   new_message.name = "Robot Status"
   new_message.level = max(levels)
   
   return new_message

    def __init__(self, *args, **kwds):
        """
        Constructor. Any message fields that are implicitly/explicitly
        set to None will be assigned a default value. The recommend
        use is keyword arguments as this is more robust to future message
        changes.  You cannot mix in-order arguments and keyword arguments.

        The available fields are:
        level,name,message,hardware_id,values

        @param args: complete set of field values, in .msg order
        @param kwds: use keyword arguments corresponding to message field names
        to set specific fields.
        """
        DiagnosticStatus.__init__(self, *args, **kwds)

def diagnostics(level, msg_short, msg_long):
    if level == 0:
        rospy.loginfo(msg_long)        
    elif level == 1:
        rospy.logwarn(msg_long)
    elif level == 2:
        rospy.logerr(msg_long)
    d = DiagnosticArray() 
    d.header.stamp = rospy.Time.now() 
    ds = DiagnosticStatus() 
    ds.level = level
    ds.message = msg_long
    ds.name = msg_short
    d.status = [ds] 
    pub_diag.publish(d)