Python DiagnosticStatus.level方法代码示例

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
 def publish_diagnostic(self):
     try:
         da = DiagnosticArray()
         ds = DiagnosticStatus()
         ds.name = rospy.get_caller_id().lstrip('/') + ': Status'
         ds.hardware_id = self.ifname
         if not self.iwconfig.is_enabled():
             ds.level = DiagnosticStatus.STALE
             ds.message = "Device not found"
         elif not self.iwconfig.is_connected():
             ds.level = DiagnosticStatus.ERROR
             ds.message = "No connection"
         else:
             if extract_number(self.iwconfig.status["Link Quality"]) < self.warn_quality:
                 ds.level = DiagnosticStatus.WARN
                 ds.message = "Connected, but bad quality"
             else:
                 ds.level = DiagnosticStatus.OK
                 ds.message = "Connected"
             for key, val in self.iwconfig.status.items():
                 ds.values.append(KeyValue(key, val))
         da.status.append(ds)
         da.header.stamp = rospy.Time.now()
         self.diagnostic_pub.publish(da)
     except Exception as e:
         rospy.logerr('Failed to publish diagnostic: %s' % str(e))
         rospy.logerr(traceback.format_exc())

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
def _camera_diag(level = 0):
    array = DiagnosticArray()
    stat = DiagnosticStatus()
    stat.name = 'wge100: Driver Status'
    stat.level = level
    stat.message = 'OK'

    motor_stat = DiagnosticStatus()
    motor_stat.name = 'EtherCAT Master'
    motor_stat.level = 0
    motor_stat.values = [
        KeyValue(key='Dropped Packets', value='0'),
        KeyValue(key='RX Late Packet', value='0')]

    mcb_stat = DiagnosticStatus()
    mcb_stat.name = 'EtherCAT Device (my_motor)'
    mcb_stat.level = 0
    mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))

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

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
 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

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
def _laptop_charge_to_diag(laptop_msg):
    rv = DiagnosticStatus()
    rv.level   = DiagnosticStatus.OK
    rv.message = 'OK'
    rv.name    = 'Laptop Battery'
    rv.hardware_id = socket.gethostname()

    if not laptop_msg.present:
        rv.level = DiagnosticStatus.ERROR
        rv.message = 'Laptop battery missing'

    charging_state = {
        SmartBatteryStatus.DISCHARGING: 'Not Charging',
        SmartBatteryStatus.CHARGING: 'Charging',
        SmartBatteryStatus.CHARGED: 'Trickle Charging'
    }.get(laptop_msg.charge_state, 'Not Charging')

    rv.values.append(KeyValue('Voltage (V)',          str(laptop_msg.voltage)))
    rv.values.append(KeyValue('Current (A)',          str(laptop_msg.rate)))
    rv.values.append(KeyValue('Charge (Ah)',          str(laptop_msg.charge)))
    rv.values.append(KeyValue('Capacity (Ah)',        str(laptop_msg.capacity)))
    rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
    rv.values.append(KeyValue('Charging State',       str(charging_state)))

    return rv

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
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)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
def diagnosticsCallback (event):

    array = DiagnosticArray()

    # Set the timestamp for diagnostics
    array.header.stamp = rospy.Time.now()
    
    motors_message = DiagnosticStatus(name = 'PhidgetMotors', level = 0,message = 'initialized', hardware_id='Phidget')
    
    if (motorsError == 1):
        motors_message.level = 2
        motors_message.message = "Phidget Motor controller can't be initialized"
        motors_message.values = [ KeyValue(key = 'Recommendation', value = motorControllerDisconnected)]
    if (motorsError == 2):
        motors_message.level = 2
        motors_message.message = "Can't set up motor speed"
        motors_message.values = [ KeyValue(key = 'Recommendation', value = motorSpeedError)]
        
    encoders_message = DiagnosticStatus(name = 'PhidgetEncoders', level = 0,message = 'initialized', hardware_id='Phidget')
    
    if (encodersError == 1):
        encoders_message.level = 2
        encoders_message.message = "Phidget Encoder board can't be initialized"
        encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderBoardDisconnected)]
    if (encodersError == 2):
        encoders_message.level = 2
        encoders_message.message = "Can't get encoder value"
        encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderValueError)]
    
    array.status = [ motors_message, encoders_message ]
  
    diagnosticPub.publish(array)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
    def _publish_diagnostic(self):
        """Publish diagnostics."""
        diagnostic = DiagnosticArray()
        diagnostic.header.stamp = rospy.Time.now()
        status = DiagnosticStatus()
        status.name = "LocalXY Origin"
        status.hardware_id = "origin_publisher"
        if self.origin is None:
            status.level = DiagnosticStatus.ERROR
            status.message = "No Origin"
        else:
            if self.origin_source == "gpsfix":
                status.level = DiagnosticStatus.OK
                status.message = "Has Origin (GPSFix)"
            elif self.origin_source == "navsat":
                status.level = DiagnosticStatus.OK
                status.message = "Has Origin (NavSatFix)"
            else:
                status.level = DiagnosticStatus.WARN
                status.message = "Origin Was Set Manually"

            frame_id = self.origin.header.frame_id
            status.values.append(KeyValue(key="Origin Frame ID", value=frame_id))

            latitude = "%f" % self.origin.pose.position.y
            status.values.append(KeyValue(key="Latitude", value=latitude))

            longitude = "%f" % self.origin.pose.position.x
            status.values.append(KeyValue(key="Longitude", value=longitude))

            altitude = "%f" % self.origin.pose.position.z
            status.values.append(KeyValue(key="Altitude", value=altitude))

            diagnostic.status.append(status)
            self.diagnostic_pub.publish(diagnostic)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
    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)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
def status_cb(msg):
    global BATTERY_STATES
    global diag_publisher
    diag_msg = DiagnosticArray()
    diag_msg.header.stamp = rospy.Time.now()
    diag_msg.status = []
    
    temp_status = DiagnosticStatus()
    temp_status.name = "Chassis Temperature"
    temp_status.hardware_id = "automow_pcb"
    temp_status.values = []
    top_F = msg.temperature_1 * 9/5 + 32
    bot_F = msg.temperature_2 * 9/5 + 32
    temp_status.values.append(KeyValue(key="Top Celsius",
                                       value="%3.2f C"%msg.temperature_1))
    temp_status.values.append(KeyValue(key="Bottom Celsius",
                                       value="%3.2f C"%msg.temperature_2))
    temp_status.values.append(KeyValue(key="Top Fahrenheit",
                                       value="%3.2f F"%(top_F)))
    temp_status.values.append(KeyValue(key="Bottom Fahrenheit",
                                       value="%3.2f F"%(bot_F)))
    if top_F > 100 or bot_F > 100:
        temp_status.message = "High Temperature"
        temp_status.level = temp_status.WARN
    elif top_F > 125 or bot_F > 125:
        temp_status.message = "Critical Temperature"
        temp_status.level = temp_status.ERROR
    else:
        temp_status.message = "OK"
        temp_status.level = temp_status.OK
    diag_msg.status.append(temp_status)

    batt_status = DiagnosticStatus()
    batt_status.name = "Battery Status"
    batt_status.hardware_id = "automow_pcb"
    batt_status.values = []
    state = BATTERY_STATES[msg.battery_state]
    batt_status.values.append(KeyValue(key="State",
                                       value=state))
    batt_status.values.append(KeyValue(key="Charge",
                                       value="{:.0%}".format(msg.charge/100.0)))
    batt_status.values.append(KeyValue(key="Voltage",
                                       value="%3.2f V"%(msg.voltage/1000.0)))
    batt_status.values.append(KeyValue(key="Battery Current",
                                       value="%3.2f A"%(msg.current/1000.0)))
    diag_msg.status.append(batt_status)
    if msg.battery_state >= 4:
        batt_status.level = batt_status.ERROR
    else:
        batt_status.level = batt_status.OK
    batt_status.message = state
    diag_publisher.publish(diag_msg)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
def mark_diag_stale(diag_stat = None, error = False):
    if not diag_stat:
        diag_stat = DiagnosticStatus()
        diag_stat.message = 'No Updates'
        diag_stat.name    = DIAG_NAME
    else:
        diag_stat.message = 'Updates Stale'

    diag_stat.level = DiagnosticStatus.WARN
    if error:
        diag_stat.level = DiagnosticStatus.ERROR

    return diag_stat

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
    def publish(self, state):
        STATE_INDEX_CHARGING = 0
        STATE_INDEX_BATTERY = 1
        STATE_INDEX_CONNECTION = 2

        ## timed gate: limit to 1 Hz
        curr_time = rospy.get_rostime()
        if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
            return
        self.last_diagnostics_time = curr_time

        ## compose diagnostics message
        diag = DiagnosticArray()
        diag.header.stamp = curr_time
        # battery info
        stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
        try:
            battery_state_code = state[STATE_INDEX_BATTERY]
            stat.message = self.STATE_TEXTS_BATTERY[battery_state_code]
            if battery_state_code < 3:
                stat.level = DiagnosticStatus.WARN
                if battery_state_code < 1:
                    stat.level = DiagnosticStatus.ERROR
                stat.message = "Please Recharge Battery (%s)." % self.STATE_TEXTS_BATTERY[battery_state_code]
        except KeyError as ex:
            stat.message = "Invalid Battery State %s"%ex
            rospy.logwarn("Invalid Battery State %s"%ex)
            stat.level = DiagnosticStatus.ERROR
        diag.status.append(stat)
        # connection info
        stat = DiagnosticStatus(name='ps3joy'": Connection Type", level=DiagnosticStatus.OK, message="OK")
        try:
            stat.message = self.STATE_TEXTS_CONNECTION[state[STATE_INDEX_CONNECTION]]
        except KeyError as ex:
            stat.message = "Invalid Connection State %s"%ex
            rospy.logwarn("Invalid Connection State %s"%ex)
            stat.level = DiagnosticStatus.ERROR
        diag.status.append(stat)
        # charging info
        stat = DiagnosticStatus(name='ps3joy'": Charging State", level=DiagnosticStatus.OK, message="OK")
        try:
            stat.message = self.STATE_TEXTS_CHARGING[state[STATE_INDEX_CHARGING]]
        except KeyError as ex:
            stat.message = "Invalid Charging State %s"%ex
            rospy.logwarn("Invalid Charging State %s"%ex)
            stat.level = DiagnosticStatus.ERROR
        diag.status.append(stat)
        # publish message
        self.diag_pub.publish(diag)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
    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)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
    def odom_cb(self, msg):
        with self.lock:
            dist = msg.distance + (msg.angle * 0.25)

            # check if base moved
            if dist > self.dist + EPS:
                self.start_time = rospy.Time.now()
                self.start_angle = self.last_angle
                self.dist = dist
        
            # do imu test if possible
            if rospy.Time.now() > self.start_time + rospy.Duration(10.0):
                self.drift = fabs(self.start_angle - self.last_angle)*180/(pi*10)
                self.start_time = rospy.Time.now()
                self.start_angle = self.last_angle
                self.last_measured = rospy.Time.now()
                
            # publish diagnostics
            d = DiagnosticArray()
            d.header.stamp = rospy.Time.now()
            ds = DiagnosticStatus()
            ds.name = "imu_node: Imu Drift Monitor"
            if self.drift < 0.5:
                ds.level = DiagnosticStatus.OK
                ds.message = 'OK'
            elif self.drift < 1.0:
                ds.level = DiagnosticStatus.WARN
                ds.message = 'Drifting'
            else:
                ds.level = DiagnosticStatus.ERROR
                ds.message = 'Drifting'
            drift = self.drift
            if self.drift < 0:
                last_measured = 'No measurements yet, waiting for base to stop moving before measuring'
                drift = 'N/A'
            else:
                age = (rospy.Time.now() - self.last_measured).to_sec()
                if age < 60:
                    last_measured = '%f seconds ago'%age
                elif age < 3600:
                    last_measured = '%f minutes ago'%(age/60)
                else:
                    last_measured = '%f hours ago'%(age/3600)                    
            ds.values = [
                KeyValue('Last measured', last_measured),
                KeyValue('Drift (deg/sec)', str(drift)) ]
            d.status = [ds]
            self.pub_diag.publish(d)

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
 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()

# 需要导入模块: from diagnostic_msgs.msg import DiagnosticStatus [as 别名]
# 或者: from diagnostic_msgs.msg.DiagnosticStatus import level [as 别名]
    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()