Python msg.DiagnosticArray类代码示例

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 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)

 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 _publish_diag_agg(self):
        msg = DiagnosticArray()
        msg.status = [
            DiagnosticStatus(level = 0, name='/Cameras', message='OK'),
            DiagnosticStatus(level = 1, name='/Cameras/wge100', message='Uh Oh'),
            DiagnosticStatus(level = 0, name='/Lasers', message='OK'),
            DiagnosticStatus(level = 2, name='/Other', message='Error')]

        msg.header.stamp = rospy.get_rostime()

        self.diag_agg_pub.publish(msg)
        self._last_diag_agg_pub = rospy.get_time()

    def publish(self, event):
        """ Publish current diagnostics status once a second.

        Runs in a separate timer thread, so locking is required.

        :param event: rospy.TimerEvent for this call
        """
        with self.lock:
            array = DiagnosticArray()
            array.header.stamp = event.current_real
            array.status = list(self.devices.values())
            self.pub.publish(array)

    def publish(self, msg):
        """Publishes a single diagnostic status or a vector of diagnostic statuses."""
        if not type(msg) is list:
            msg = [msg]

        for stat in msg:
            stat.name = rospy.get_name()[1:]+ ": " + stat.name

        da = DiagnosticArray()
        da.status = msg
        da.header.stamp = rospy.Time.now() # Add timestamp for ROS 0.10
        self.publisher.publish(da)

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)

def make_status_msg(count):
    array = DiagnosticArray()
    stat = DiagnosticStatus()
    stat.level = 0
    stat.message = 'OK'
    stat.name = 'Unit Test'
    stat.hardware_id = 'HW ID'
    stat.values = [ 
        KeyValue('Value A', str(count)),
        KeyValue('Value B', str(count)),
        KeyValue('Value C', str(count))]
    array.status = [ stat ]
    return array

 def transform(self, msg):
   subtopic = self.params
   statuses = msg.status
   for status in statuses:
       if not status.name.startswith('/'):
           status.name = '/' + status.name
 
   def device(status): return status.name.startswith(subtopic)
   filtered_statuses = filter(device, statuses)
     
   new_message = DiagnosticArray()
   new_message.status = filtered_statuses
  
   return new_message

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) 

def publish(pub, info):
    diag = DiagnosticArray()
    diag.header.stamp = rospy.Time.now()
    status = DiagnosticStatus()
    status.hardware_id = "wifi"
    status.name = 'wifi_scan'
    status.level = status.OK
    status.message = pformat(info)
    
    for k,v in info.items():
        status.values.append(
            KeyValue(k,str(v)),
        )
    diag.status = [status]
    pub.publish(diag)

    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)

def publish():
	sensor = modbus(method='rtu', port='/dev/ucfsub/depth', baudrate=115200)
	sensor.connect()

	rospy.init_node('Depth')
	tempPub = rospy.Publisher('ExternalTemperature', Temperature, queue_size=1)
	depthPub = rospy.Publisher('/depth', Float32, queue_size=1)
	posePub = rospy.Publisher('/depth/pose', PoseWithCovarianceStamped, queue_size=1)
	diagPub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)

	temp = Temperature()
	depth = Float32()
	diag = DiagnosticArray()

	updateRate = rospy.get_param("/updateRate", 30)
	freq = rospy.Rate(updateRate)
	loop = 0

	pose = PoseWithCovarianceStamped()
	pose.header.frame_id = "odom"
	pose.pose.covariance = [0.0]*36
	pose.pose.covariance[14] = 0.01
	pose.pose.pose.orientation.x = 0.0
	pose.pose.pose.orientation.y = 0.0
	pose.pose.pose.orientation.z = 0.0
	pose.pose.pose.orientation.w = 1.0
	pose.pose.pose.position.x = 0.0
	pose.pose.pose.position.y = 0.0

	while not rospy.is_shutdown():
		if loop >= updateRate:
			rr = sensor.read_holding_registers(address=8, count=2, unit=1)
			if type(rr) is not type(ModbusIOException):
				temp.temperature = struct.unpack('>f',struct.pack('>HH', *rr.registers))[0]
				tempPub.publish(temp)
			loop = 0
		loop += 1

		rr = sensor.read_holding_registers(address=2, count=2, unit=1)
		if type(rr) is not type(ModbusIOException):
			depth.data = 10.2*struct.unpack('>f',struct.pack('>HH', *rr.registers))[0]

			pose.pose.pose.position.z = depth.data
			pose.header.stamp = rospy.Time.now()
			posePub.publish(pose)
			depthPub.publish(depth)
		diag.status = [DiagnosticStatus(name='Depth', message=str(type(rr) is type(ModbusIOException)), level=int(rr == 'true')*2)]
		freq.sleep()

 def callback(self, _msg):
     msg = DiagnosticArray()
     statuses = []
     for _status in _msg.status:
         status = DiagnosticStatus()
         status.level = _status.level
         status.name = _status.name
         if _status.code == 0:
             status.message = ""
         else:
             status.message = self.status_codes_by_module[_status.name].get(
                 _status.code, "Unknown error"
             )
         statuses.append(status)
     msg.status = statuses
     self.pub.publish(msg)

    def test_add_agg(self):
        self.wait_for_agg()

        # confirm that the things we're going to add aren't there already
        with self._mutex:
            agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()]
            self.assert_(not any(expected in agg_paths for expected in self.expected))
            
        # add the new groups
        self.add_analyzer()

        arr = DiagnosticArray()
        arr.header.stamp = rospy.get_rostime()
        arr.status = [
            DiagnosticStatus(name='primary', message='hello-primary'),
            DiagnosticStatus(name='secondary', message='hello-secondary')
        ]
        self.pub.publish(arr)
        self.wait_for_agg()
        # the new aggregator data should contain the extra paths. At this point
        # the paths are probably still in the 'Other' group because the bond
        # hasn't been fully formed
        with self._mutex:
            agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()]
            self.assert_(all(expected in agg_paths for expected in self.expected))

        rospy.sleep(rospy.Duration(5)) # wait a bit for the new items to move to the right group
        arr.header.stamp = rospy.get_rostime()
        self.pub.publish(arr) # publish again to get the correct groups to show OK
        self.wait_for_agg()

        for name, msg in self.agg_msgs.iteritems():
            if name in self.expected: # should have just received messages on the analyzer
                self.assert_(msg.message == 'OK')
                
            agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()]
            self.assert_(all(expected in agg_paths for expected in self.expected))
                

        self.bond.shutdown()
        self.wait_for_agg()
        # the aggregator data should no longer contain the paths once the bond is shut down
        with self._mutex:
            agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()]
            self.assert_(not any(expected in agg_paths for expected in self.expected))