Exemple #1
0
 def _publish_diagnostics(self):
     pilot = self._get_pilot()
     if pilot:
         status = DiagnosticStatus(name='mode', message='not_available')
         status.values = [
             KeyValue(key='steer',
                      value='{:+2.5f}'.format(pilot.get('steering'))),
             KeyValue(key='throttle',
                      value='{:+2.5f}'.format(pilot.get('throttle')))
         ]
         driver_mode = pilot.get('driver')
         if driver_mode == 'driver_mode.teleop.direct':
             status.message = 'teleoperation'
         elif driver_mode == 'driver_mode.inference.dnn':
             status.message = 'autopilot'
             status.values += [
                 KeyValue(key='maximum_speed',
                          value='{:+2.5f}'.format(
                              pilot.get('cruise_speed'))),
                 KeyValue(key='desired_speed',
                          value='{:+2.5f}'.format(
                              pilot.get('desired_speed')))
             ]
         diagnostics = DiagnosticArray()
         diagnostics.header.stamp = self.get_clock().now().to_msg()
         diagnostics.status = [status]
         self._ros_publisher.publish(diagnostics)
Exemple #2
0
 def publish_diagnostic_msg(self):
     """Helper to imu_callback. Publishes a DiagnosticArray containing
     calibration information.
     """
     diag_msg = DiagnosticArray()
     diag_msg.header.stamp = rospy.Time.now()
     diag_msg.status = [
         DiagnosticStatus(level=DiagnosticStatus.OK,
                          name='IMU Calibration Info',
                          values=[
                              KeyValue(key='Gyro Bias',
                                       value=str(self.gyro_bias)),
                              KeyValue(key='Gyro Scale',
                                       value=str(self.gyro_scale)),
                              KeyValue(key='Accel Offsets',
                                       value=str(self.acc_offsets)),
                              KeyValue(key='Accel Transform',
                                       value=str(self.acc_transform)),
                              KeyValue(key='Mag Offsets',
                                       value=str(self.mag_offsets)),
                              KeyValue(key='Mag Transform',
                                       value=str(self.mag_transform)),
                              KeyValue(key='Misalignment',
                                       value=str(self.misalignment))
                          ])
     ]
     self.imu_diag_pub.publish(diag_msg)
     return
Exemple #3
0
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)
Exemple #4
0
 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()
Exemple #5
0
    def _on_packet_pong(self, packet):
        """
        Re-publishes the pong responses as a diagnostic message.
        """

        try:
            packet_dict = self.get_packet_dict(packet)
            if not packet_dict:
                return
        except (ValueError, TypeError) as e:
            return

        self.last_pong = time.time()
        self.last_pong_dt = datetime.now()
        total = packet_dict['total']
        #print('pong total:', total)

        array = DiagnosticArray()
        pong_status = DiagnosticStatus(name='%s Arduino' % self.name.title(),
                                       level=OK)
        pong_status.values = [
            KeyValue(key='total', value=str(total)),
        ]
        array.status = [
            pong_status,
        ]
        self.diagnostics_pub.publish(array)
Exemple #6
0
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)
Exemple #7
0
 def filter_diagnostics_agg(self, diagnostics_agg):
     diagnostics_agg_error_only = DiagnosticArray()
     diagnostics_agg_error_only.header = diagnostics_agg.header
     for status in diagnostics_agg.status:
         if status.level != DiagnosticStatus.OK:
             diagnostics_agg_error_only.status.append(status)
     return diagnostics_agg_error_only
Exemple #8
0
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)
Exemple #9
0
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 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()
Exemple #11
0
def state_cb(msg):
    global last_publish_time
    now = rospy.get_rostime()
    if (now - last_publish_time).to_seconds() > 1.0:
        d = DiagnosticArray()
        d.header.stamp = msg.header.stamp
        d.status = [joint_to_diag(js) for js in msg.joint_states]
        pub_diag.publish(d)
        last_publish_time = now
Exemple #12
0
def wrapper(jetson):
    # Load level options
    level_options = {
        rospy.get_param("~level/error", 60): DiagnosticStatus.ERROR,
        rospy.get_param("~level/warning", 40): DiagnosticStatus.WARN,
        rospy.get_param("~level/ok", 20): DiagnosticStatus.OK,
    }
    rospy.loginfo(level_options)
    # Initialization ros pubblisher
    pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
    # Set default rate jetson stats 2hz
    rate_node = rospy.get_param("~rate", 2)
    rate = rospy.Rate(rate_node)
    # Extract board information
    board = jetson.board
    # Define hardware name
    hardware = board["info"]["Machine"]
    # Define Diagnostic array message
    # http://docs.ros.org/api/diagnostic_msgs/html/msg/DiagnosticStatus.html
    arr = DiagnosticArray()
    # Initialization Tegrastats
    while not rospy.is_shutdown():
        # Save all stats
        stats = jetson.stats
        # Add timestamp
        arr.header.stamp = rospy.Time.now()
        # Status board and board info
        arr.status = [other_status(hardware, jetson)]
        # Make diagnostic message for each cpu
        arr.status += [cpu_status(hardware, cpu) for cpu in stats['CPU']]
        # Merge all other diagnostics
        if 'GR3D' in stats:
            arr.status += [gpu_status(hardware, stats['GR3D'])]
        if 'RAM' in stats:
            arr.status += [ram_status(hardware, stats['RAM'], 'mem')]
        if 'SWAP' in stats:
            arr.status += [swap_status(hardware, stats['SWAP'], 'mem')]
        if 'EMC' in stats:
            arr.status += [emc_status(hardware, stats['EMC'], 'mem')]
        if 'FAN' in stats:
            arr.status += [fan_status(hardware, stats['FAN'], 'board')]
        if 'WATT' in stats:
            arr.status += [power_status(hardware, stats['WATT'])]
        if 'TEMP' in stats:
            arr.status += [temp_status(hardware, stats['TEMP'], level_options)]
        # Status board and board info
        arr.status += [board_status(hardware, board, 'board')]
        # Add disk status
        arr.status += [disk_status(hardware, jetson.disk, 'board')]
        # Update status jtop
        rospy.logdebug("jtop message %s" % rospy.get_time())
        pub.publish(arr)
        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, 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)
Exemple #15
0
    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)
Exemple #16
0
    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)
Exemple #17
0
    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)
Exemple #18
0
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)
Exemple #19
0
    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()
        rospy.sleep(
            rospy.Duration(5))  # wait a bit for the analyzers to unload
        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))
Exemple #20
0
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 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
Exemple #22
0
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)
Exemple #23
0
    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)
Exemple #24
0
 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
Exemple #25
0
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 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) 
Exemple #27
0
 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)
Exemple #28
0
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 _diag(ID, category, state, msg='', key_value={}):

    array = DiagnosticArray()
    full_name = "mcr_diag" + rospy.get_name() + "/" + ID + "/"
    full_name = full_name.replace("//", "/")
    s1 = DiagnosticStatus(name=full_name, level=state, message=msg)
    values = []
    for k, v in key_value.items():
        values.append(KeyValue(key=k, value=v))
    values.append(KeyValue(key='failure_category', value=category))
    s1.values = values
    array.status = [s1]
    _pub.publish(array)
    if (s1.level != _STATUS_OK):
        diag_msgs.append(s1)
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 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)
Exemple #32
0
    def _publish(self, time):
        """ Publishes diagnostic data"""
        # Limit to 1 Hz
        if (time - self.last_diagnostics_time).to_sec() < 1.0:
            return
        self.last_diagnostics_time = time
        diag = DiagnosticArray()
        diag.header.stamp = time

        # E-Stop
        if self.stat_estop:
            diag.status.append(self.stat_estop)
        # Power
        if self.stat_power:
            diag.status.append(self.stat_power)
        # Voltage
        if self.stat_voltage:
            diag.status.append(self.stat_voltage)
        # Temperature
        if self.stat_temp:
            diag.status.append(self.stat_temp)
        # Uptime
        if self.stat_uptime:
            diag.status.append(self.stat_uptime)

        # Publish
        print diag
        self.diag_pub.publish(diag)
 def diagnostics(self, state):
     try:
         da = DiagnosticArray()
         ds = DiagnosticStatus()
         ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
         if state == 0:
             ds.level = DiagnosticStatus.OK
             ds.message = "%i sounds playing" % self.active_sounds
             ds.values.append(
                 KeyValue("Active sounds", str(self.active_sounds)))
             ds.values.append(
                 KeyValue("Allocated sound channels",
                          str(self.num_channels)))
             ds.values.append(
                 KeyValue("Buffered builtin sounds",
                          str(len(self.builtinsounds))))
             ds.values.append(
                 KeyValue("Buffered wave sounds",
                          str(len(self.filesounds))))
             ds.values.append(
                 KeyValue("Buffered voice sounds",
                          str(len(self.voicesounds))))
         elif state == 1:
             ds.level = DiagnosticStatus.WARN
             ds.message = "Sound device not open yet."
         else:
             ds.level = DiagnosticStatus.ERROR
             ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting"
         da.status.append(ds)
         da.header.stamp = rospy.get_rostime()
         self.diagnostic_pub.publish(da)
     except Exception, e:
         rospy.loginfo('Exception in diagnostics: %s' % str(e))
Exemple #34
0
    def update(self):
        with self._mutex:
            diag = DiagnosticArray()
            diag.header.stamp = rospy.get_rostime()
            
            info_update_ok  = rospy.get_time() - self._last_info_update  < 5.0 / self._batt_info_rate
            state_update_ok = rospy.get_time() - self._last_state_update < 5.0 / self._batt_state_rate

            if info_update_ok:
                self._msg.design_capacity = self._batt_design_capacity
                self._msg.capacity        = self._batt_last_full_capacity
            else:
                self._msg.design_capacity = 0.0
                self._msg.capacity        = 0.0
                
            if info_update_ok and state_update_ok and self._msg.capacity != 0:
                self._msg.percentage = int(self._msg.charge / self._msg.capacity * 100.0)

            diag_stat = _laptop_charge_to_diag(self._msg)
            if not info_update_ok or not state_update_ok:
                diag_stat.level   = DiagnosticStatus.ERROR
                diag_stat.message = 'Laptop battery data stale'

            diag.status.append(diag_stat)

            self._diag_pub.publish(diag)
            self._power_pub.publish(self._msg)
Exemple #35
0
    def send_qa_value_msgs(self, timeout=5.0):
        ##############################################################################

        rospy.init_node(NAME, anonymous=True)
        self.message_pub = rospy.Publisher('/diagnostics',
                                           DiagnosticArray,
                                           queue_size=1)
        rospy.Subscriber("/rosout", Log, self.log_callback)

        timeout_t = time.time() + timeout  # Default timeout 5 sec.

        while not rospy.is_shutdown(
        ) and not self.success and time.time() < timeout_t:

            diag_msg = DiagnosticArray()
            diag_msg.header.stamp = rospy.get_rostime()
            status_msg = DiagnosticStatus()
            status_msg.level = DiagnosticStatus.OK
            status_msg.name = "fg_print"
            status_msg.values.append(KeyValue(str(QA_TYPE), str(QA_VALUE)))
            status_msg.message = "QA status"
            diag_msg.status.append(status_msg)

            self.message_pub.publish(diag_msg)
            time.sleep(MSG_DELAY)
 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)
Exemple #37
0
class NVidiaTempMonitor(object):
    def __init__(self, hostname):
        self._pub = rospy.Publisher('/diagnostics', DiagnosticArray)
        self._gpu_pub = rospy.Publisher('gpu_status', GPUStatus)
        self.xmlMode = rospy.get_param('~/xml', False)
        self.hostname = hostname

    def pub_status(self):
        gpu_stat = GPUStatus()
        stat = DiagnosticStatus()
        try:
            card_out = computer_monitor.get_gpu_status(xml=True)
            gpu_stat = computer_monitor.parse_smi_output(card_out)
            stat = computer_monitor.gpu_status_to_diag(gpu_stat, self.hostname)
        except Exception, e:
            import traceback
            rospy.logerr('Unable to process nVidia GPU data')
            rospy.logerr(traceback.format_exc())
            stat.name = '%s GPU Status' % self.hostname
            stat.message = 'Could not get GPU information'
            stat.level = DiagnosticStatus.ERROR

        gpu_stat.header.stamp = rospy.get_rostime()

        array = DiagnosticArray()
        array.header.stamp = rospy.get_rostime()

        array.status = [ stat ]

        self._pub.publish(array)
        self._gpu_pub.publish(gpu_stat)
Exemple #38
0
    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)
    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 update_diagnostics(self):
     da = DiagnosticArray()
     ds = DiagnosticStatus()
     ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks"
     in_progress = 0
     longest_interval = 0
     for updater in list(asynchronous_updaters):
         (name, interval) = updater.getStatus()
         if interval == 0:
             msg = "Idle"
         else:
             in_progress = in_progress + 1
             msg = "Update in progress (%i s)" % interval
         longest_interval = max(interval, longest_interval)
         ds.values.append(KeyValue(name, msg))
     if in_progress == 0:
         ds.message = "Idle"
     else:
         ds.message = "Updates in progress: %i" % in_progress
     if longest_interval > 10:
         ds.level = 1
         ds.message = "Update is taking too long: %i" % in_progress
     ds.hardware_id = "none"
     da.status.append(ds)
     da.header.stamp = rospy.get_rostime()
     self.diagnostic_pub.publish(da)
Exemple #41
0
    def publish_status(self):
        status = 0
        messages = []

        array = DiagnosticArray()
        for listener in self._listeners:
            stat, msg, diags = listener.check_ok()
            status = max(status, stat)
            if msg != '':
                messages.append(msg)
            if diags:
                array.status.extend(diags)

        if len(self._listeners) == 0:
            status = 3
            messages = ['No listeners']

        if len(array.status) > 0:
            self._diag_pub.publish(array)

        test_stat = TestStatus()
        test_stat.test_ok = int(status)
        test_stat.message = ', '.join(messages)
        if test_stat.test_ok == 0:
            test_stat.message = 'OK'

        self._status_pub.publish(test_stat)
Exemple #42
0
    def __init__(self):
        super().__init__('jtop_publisher')
        self.publisher_ = self.create_publisher(DiagnosticArray, 'diagnostics',
                                                1)
        # Create Services
        self.fan_srv = self.create_service(Fan, '/jtop/fan', self.fan_service)
        self.nvpmodel_srv = self.create_service(NVPModel, '/jtop/nvpmodel',
                                                self.nvpmodel_service)
        self.jetson_clocks_srv = self.create_service(
            JetsonClocks, '/jtop/jetson_clocks', self.jetson_clocks_service)
        self.declare_parameter('interval', 0.5)  # Default interval 0.5
        self.declare_parameter('level_error', 60)  # Default interval 0.5
        self.declare_parameter('level_warning', 40)  # Default interval 0.5
        self.declare_parameter('level_ok', 20)  # Default interval 0.5

        self.level_options = {
            self.get_parameter('level_error')._value: DiagnosticStatus.ERROR,
            self.get_parameter('level_warning')._value: DiagnosticStatus.WARN,
            self.get_parameter('level_ok')._value: DiagnosticStatus.OK,
        }

        timer_period = self.get_parameter('interval')._value
        self.timer = self.create_timer(timer_period, self.jetson_callback)
        self.i = 0
        self.jetson = jtop.jtop(interval=0.5)
        self.arr = DiagnosticArray()
        self.get_logger().info(
            "Jetson Stats has started with interval : {}\n You can run following:\n  1. $ros2 run rqt_topic rqt_topic \n  2. Services for controlling fan_speed, power_mode, jetson_clocks\n"
            .format(timer_period))
def make_diagnostic(origin, static_origin):
    diagnostic = DiagnosticArray()
    diagnostic.header.stamp = rospy.Time.now()
    status = DiagnosticStatus()
    status.name = "LocalXY Origin"
    status.hardware_id = "origin_publisher"
    if origin == None:
        status.level = DiagnosticStatus.ERROR
        status.message = "No Origin"
    else:
        if not static_origin:
            status.level = DiagnosticStatus.OK
            status.message = "Has Origin (auto)"
        else:
            status.level = DiagnosticStatus.WARN
            status.message = "Origin is static (non-auto)"
        
        frame_id = origin.header.frame_id
        status.values.append(KeyValue(key="Origin Frame ID", value=frame_id))
        
        latitude = "%f" % origin.pose.position.y
        status.values.append(KeyValue(key="Latitude", value=latitude))
        
        longitude = "%f" % origin.pose.position.x
        status.values.append(KeyValue(key="Longitude", value=longitude))
        
        altitude = "%f" % origin.pose.position.z
        status.values.append(KeyValue(key="Altitude", value=altitude))
    
    diagnostic.status.append(status)
    return diagnostic
    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))
Exemple #45
0
  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)

    #rospy.loginfo("statuses: %s" % statuses)
    #rospy.loginfo("f_statuses: %s" % filtered_statuses)

    #for status in filtered_statuses:
    #    rospy.loginfo("name: %s" % status.name)
    new_message = DiagnosticArray()
    new_message.status = filtered_statuses
    
    return new_message
def publish():
	rospy.init_node('Leak')
	leakPub = rospy.Publisher('/leak', Bool, queue_size=1)
	diagPub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
	#GPIO.setmode(GPIO.BCM)
	#GPIO.setup(CHANNEL, GPIO.IN)
	gpio.setup(CHANNEL, gpio.IN)
	freq = rospy.Rate(1)
	leak = Bool()
	diag = DiagnosticArray()
	while not rospy.is_shutdown():
		levelData=0
		leak.data = gpio.input(CHANNEL)
		if (leak.data):
			levelData=2
		diag.status = [DiagnosticStatus(name='Leak', message=str(leak.data), level=levelData)]
		leakPub.publish(leak)
		diagPub.publish(diag)
		freq.sleep()
Exemple #47
0
    def publish_diags(self, strategy):
        now = rospy.get_rostime()
        ns = strategy.ns
        ds = self.fill_diags("synthetic interface", ns.diag_summary, self.hostname, strategy.ns.diags)
        ds.level = ns.diag_level
        statuses = [ds]

        for i in range(0, len(ns.interfaces)):
            iface = ns.interfaces[i]
            ds = self.fill_diags(iface.iface, iface.diag_summary, self.hostname, iface.diags)
            statuses.append(ds)
            if iface.__class__ == mir.WirelessInterface:
                msg = self.gen_accesspoint_msg(iface)
                msg.header.stamp = now
                self.wifi_sub_pub[iface.iface].publish(msg)
                if i == ns.active_iface:
                    self.wifi_pub.publish(msg)

        da = DiagnosticArray()
        da.header.stamp = rospy.get_rostime()
        da.status = statuses
        self.diag_pub.publish(da)
 def update(self):
     msg = DiagnosticArray()
     msg.header.stamp = rospy.Time.now()
     msg.status = []
     it = 0
     while it < 20:
         status = DiagnosticStatus()
         status.level = random.randint(0,2)
         status.name = "Test %i"%it
         status.hardware_id = "Dummy Diagnostics"
         if status.level == 0:
             message = "OK"
         elif status.level == 1:
             message = "WARN"
         elif status.level == 2:
             message = "ERROR"
         status.message = message
         status.values = []
         ii = 0
         while ii < 20:
             status.values.append(KeyValue("Key %i"%ii,str(random.randint(0,50))))
             ii += 1
         it += 1
         msg.status.append(status)
     self.publisher.publish(msg)
     msg = DiagnosticArray()
     msg.header.stamp = rospy.Time.now()
     msg.status = []
     status = DiagnosticStatus()
     status.level = status.WARN
     status.name = "Test Warn"
     status.hardware_id = "Dummy Diagnostics"
     status.message = "Warning - This is a test"
     status.values = []
     msg.status.append(status)
     self.publisher.publish(msg)
        DiagnosticStatus(0, '/BASE/group1/item2', 'OK', '', []),

        DiagnosticStatus(0, '/BASE/group2', 'OK', '', []),
        DiagnosticStatus(0, '/BASE/group2/item1', 'OK', '', []),

        DiagnosticStatus(2, '/BASE2', 'OK', '', []),
        DiagnosticStatus(2, '/BASE2/group3', 'OK', '', []),
        DiagnosticStatus(2, '/BASE2/group3/item2', 'Error', '', []),

        #DiagnosticStatus(0, '/BASE3', 'OK', '', []),
        DiagnosticStatus(0, '/BASE3/group1', 'OK', '', []),
        DiagnosticStatus(0, '/BASE3/group1/item2', 'Error', '', [])
        ]
    ]
    
    array = DiagnosticArray() 
    
    array.header.stamp = rospy.get_rostime()

    i = 0
    ind = 0
    while not rospy.is_shutdown():
        old_ind = ind
        ind = 0
        if (i % 10 > 5):
            ind = 1 
        i = i + 1
        
        if (old_ind != ind):
            print "switched", ind
        
 def __publish_diagnostic_information(self):
     diag_msg = DiagnosticArray()
     
     rate = rospy.Rate(self.diagnostics_rate)
     while not rospy.is_shutdown() and self.running:
         diag_msg.status = []
         diag_msg.header.stamp = rospy.Time.now()
         
         status = DiagnosticStatus()
         
         status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace
         status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name
         status.values.append(KeyValue('Baud Rate', str(self.baud_rate)))
         status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id)))
         status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id)))
         status.values.append(KeyValue('Desired Update Rate', str(self.update_rate)))
         status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate)))
         status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal'])))
         status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum'])))
         status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped'])))
         status.level = DiagnosticStatus.OK
         status.message = 'OK'
         
         if self.actual_rate - self.update_rate < -5:
             status.level = DiagnosticStatus.WARN
             status.message = 'Actual update rate is lower than desired'
             
         diag_msg.status.append(status)
         
         for motor_state in self.current_state.motor_states:
             mid = motor_state.id
             
             status = DiagnosticStatus()
             
             status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace)
             status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace)
             status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model'])))
             status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware'])))
             status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay'])))
             status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage'])))
             status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage'])))
             status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle'])))
             status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle'])))
             
             status.values.append(KeyValue('Goal', str(motor_state.goal)))
             status.values.append(KeyValue('Position', str(motor_state.position)))
             status.values.append(KeyValue('Error', str(motor_state.error)))
             status.values.append(KeyValue('Velocity', str(motor_state.speed)))
             status.values.append(KeyValue('Load', str(motor_state.load)))
             status.values.append(KeyValue('Voltage', str(motor_state.voltage)))
             status.values.append(KeyValue('Temperature', str(motor_state.temperature)))
             status.values.append(KeyValue('Moving', str(motor_state.moving)))
             
             if motor_state.temperature >= self.error_level_temp:
                 status.level = DiagnosticStatus.ERROR
                 status.message = 'OVERHEATING'
             elif motor_state.temperature >= self.warn_level_temp:
                 status.level = DiagnosticStatus.WARN
                 status.message = 'VERY HOT'
             else:
                 status.level = DiagnosticStatus.OK
                 status.message = 'OK'
                 
             diag_msg.status.append(status)
             
         self.diagnostics_pub.publish(diag_msg)
         rate.sleep()
def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname = None, error_offset = 5000000):
    pub = rospy.Publisher("/diagnostics", DiagnosticArray)
    rospy.init_node(NAME, anonymous=True)
    
    hostname = socket.gethostname()
    if diag_hostname is None:
        diag_hostname = hostname

    stat = DiagnosticStatus()
    stat.level = 0
    stat.name = "NTP offset from "+ diag_hostname + " to " + ntp_hostname
    stat.message = "OK"
    stat.hardware_id = hostname
    stat.values = []

    self_stat = DiagnosticStatus()
    self_stat.level = DiagnosticStatus.OK
    self_stat.name = "NTP self-offset for "+ diag_hostname
    self_stat.message = "OK"
    self_stat.hardware_id = hostname
    self_stat.values = []
    
    while not rospy.is_shutdown():
        for st,host,off in [(stat,ntp_hostname,offset), (self_stat, hostname,self_offset)]:
            try:
                p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE)
                res = p.wait()
                (o,e) = p.communicate()
            except OSError, (errno, msg):
                if errno == 4:
                    break #ctrl-c interrupt
                else:
                    raise
            if (res == 0):
                measured_offset = float(re.search("offset (.*),", o).group(1))*1000000

                st.level = DiagnosticStatus.OK
                st.message = "OK"
                st.values = [ KeyValue("Offset (us)", str(measured_offset)),
                              KeyValue("Offset tolerance (us)", str(off)),
                              KeyValue("Offset tolerance (us) for Error", str(error_offset)) ]
            
                if (abs(measured_offset) > off):
                    st.level = DiagnosticStatus.WARN
                    st.message = "NTP Offset Too High"
                if (abs(measured_offset) > error_offset):
                    st.level = DiagnosticStatus.ERROR
                    st.message = "NTP Offset Too High"
                                
            else:
                st.level = DiagnosticStatus.ERROR
                st.message = "Error Running ntpdate. Returned %d" % res
                st.values = [ KeyValue("Offset (us)", "N/A"),
                              KeyValue("Offset tolerance (us)", str(off)),
                              KeyValue("Offset tolerance (us) for Error", str(error_offset)),
                              KeyValue("Output", o),
                              KeyValue("Errors", e) ]


        msg = DiagnosticArray()
        msg.header.stamp = rospy.get_rostime()
        msg.status = [stat, self_stat]
        pub.publish(msg)
        time.sleep(1)
Exemple #52
0
    def _publish_status(self):
        now = rospy.get_rostime()

        # current_iface_id
        ai = self.interface_selector.active_interfaces
        if not ai or ai[0] not in self._interfaces:
            index = -1
        else:
            index = self._interfaces.index(ai[0])
        self.iface_id_pub.publish(index)

        # accesspoint
        best_active = self.interface_selector.radio_manager.best_active
        for iface in self._wireless_interfaces:
            msg = self.gen_accesspoint_msg(iface)
            msg.header.stamp = now
            self.all_ap_pub[iface].publish(msg)
            if iface == best_active:
                self.ap_pub.publish(msg)
        if best_active is None:
            self.ap_pub.publish(AccessPoint())

        # status
        msg = MultiInterfaceStatus()
        for iface in self._interfaces:
            msg.interfaces.append(self.gen_status_msg(iface))
        self.status_pub.publish(msg)

        # diagnostics
        diags = []
        diags.append(('Tunnel Interface', self.interface_selector.tunnel_interface))
        if self.interface_selector.active_interfaces and \
             self.interface_selector.active_interfaces[0].score != self.interface_selector.TERRIBLE_INTERFACE:
            act_iface = self.interface_selector.active_interfaces[0]
            diags.append(('Active Interface', act_iface.iface ))
            diags += act_iface.diags
            if act_iface.goodness > 95:
                diag_summary = "Active interface %s running strong"%act_iface.iface
                diag_level = 0
            elif act_iface.goodness > 50:
                diag_summary = "Active interface %s is lossy"%act_iface.iface
                diag_level = 1
            else:
              if act_iface.goodness > 0:
                diag_summary = "Active interface %s is very poor"%act_iface.iface
              else:
                diag_summary = "Active interface %s is failing to ping"%act_iface.iface
              diag_level = 2
        else:
            diags.append(('Active Interface', "none"))
            diag_summary = 'No active interface'
            diag_level = 2
        ds = self.fill_diags("synthetic interface", diag_summary, self.hostname, diags)
        ds.level = diag_level
        statuses = [ds]

        for iface in self._interfaces:
            status = iface.status
            if status == InterfaceStatus.STATE_ADDR and iface.ping_loss < 100:
                status = InterfaceStatus.STATE_PINGING
                diag_summary = "Connected (goodness %f, reliability %f)"%(iface.goodness, iface.reliability)
            else:
                diag_summary = STATUSES[status]
            ds = self.fill_diags(iface.iface, diag_summary, self.hostname, iface.diags)
            statuses.append(ds)

        da = DiagnosticArray()
        da.header.stamp = rospy.get_rostime()
        da.status = statuses
        self.diag_pub.publish(da)
#!/usr/bin/env python

import roslib; roslib.load_manifest('maggie_eyelids_analyzer')

import rospy
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

if __name__ == '__main__':
    rospy.init_node('eyelids_node_main')

    pub = rospy.Publisher('/diagnostics', DiagnosticArray)
    
    array = DiagnosticArray()
    # Right eyelid
    eye_right_stat = DiagnosticStatus(name = 'Right Eyelid', level = 0, 
                                  message = 'Connected')
    #Left eyelid
    eye_left_stat = DiagnosticStatus(name='Left Eyelid', level = 0,
                                message = 'Connected')

    array.status = [ eye_right_stat, eye_left_stat ]

    my_rate = rospy.Rate(1.0)
    while not rospy.is_shutdown():
        pub.publish(array)
        my_rate.sleep()
import roslib; roslib.load_manifest(PKG)

import rospy
from time import sleep

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus

if __name__ == '__main__':
    rospy.init_node('diag_pub')
    pub = rospy.Publisher('/diagnostics', DiagnosticArray)
    
    start_time = rospy.get_time()

    while not rospy.is_shutdown():
        array = DiagnosticArray()
        array.status = [
            DiagnosticStatus(0, 'EtherCAT Device (fl_caster_l_wheel_motor)', 'OK', '', []),
            DiagnosticStatus(0, 'EtherCAT Device (fl_caster_r_wheel_motor)', 'OK', '', []),
            DiagnosticStatus(0, 'EtherCAT Device (fl_caster_rotation_motor)', 'OK', '', []),
            DiagnosticStatus(2, 'EtherCAT Device (fr_caster_l_wheel_motor)', 'Motor model', '', []),
            DiagnosticStatus(1, 'EtherCAT Device (fr_caster_r_wheel_motor)', 'High temperature', '', []),
            DiagnosticStatus(0, 'EtherCAT Device (fr_caster_rotation_motor)', 'OK', '', []),
            
            DiagnosticStatus(0, 'tilt_hokuyo_node: Frequency Status', 'OK', '', []),
            DiagnosticStatus(0, 'tilt_hokuyo_node: Connection Status', 'OK', '', []),
            DiagnosticStatus(0, 'base_hokuyo_node: Frequency Status', 'OK', '', []),
            DiagnosticStatus(0, 'base_hokuyo_node: Connection Status', 'OK', '', []),
            
            DiagnosticStatus(0, 'Joint (fl_caster_l_wheel_joint)', 'OK', '', []),
            DiagnosticStatus(0, 'Joint (fl_caster_r_wheel_joint)', 'OK', '', []),
Exemple #55
0
import roslib; roslib.load_manifest(PKG)


import rospy
from time import sleep

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

if __name__ == '__main__':

    nodeName = 'tasmania_ActCap1Func2'
    rospy.init_node('diag_'+nodeName+'_pub2')
    pub = rospy.Publisher('/diagnostics', DiagnosticArray)    
     
    
    array = DiagnosticArray()
    array.status = [
        # GenericAnalyzer prefix1 
        #DiagnosticStatus(0, nodeName, 'msg: all OK', '', [KeyValue('internal_state_valTEST', '4')]),
        #DiagnosticStatus(0, nodeName, 'msg: all OK', '', [KeyValue('CompNotifierType', 'CompHeartBeatInterval'), KeyValue('Interval', '700') ]),
        DiagnosticStatus(0, nodeName, 'msg: xxx', '', [KeyValue('CharacType', 'CompCpu'), KeyValue('Measurement', '300') ]),
        #DiagnosticStatus(0, nodeName, 'msg: all OK', '', [KeyValue('CharacType', 'CompMem'), KeyValue('Measurement', '3000') ])
        ]
    #array.header.stamp = rospy.get_rostime()
    array.header.stamp = rospy.Time.now()
    #print rospy.Time.now()
    #print rospy.get_rostime()

    count = 0
    while not rospy.is_shutdown():
        array.header.stamp = rospy.get_rostime()
import roslib; roslib.load_manifest(PKG)


import rospy
from time import sleep

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus

if __name__ == '__main__':
    rospy.init_node('diag_pub')
    pub = rospy.Publisher('/diagnostics', DiagnosticArray)
    
    start_time = rospy.get_time()
    
    while not rospy.is_shutdown():
        array = DiagnosticArray()
        array.header.stamp = rospy.get_rostime()
        array.status = [
            # GenericAnalyzer my_path
            DiagnosticStatus(0, 'multi', 'OK', '', []),
            DiagnosticStatus(1, 'Something', 'OK', '', []),
            DiagnosticStatus(2, 'Something Else', 'OK', '', []),
            
            # OtherAnalyzer for Other
            DiagnosticStatus(2, 'other2', 'OK', '', []),
            DiagnosticStatus(0, 'other3', 'OK', '', [])]
        
        pub.publish(array)
        sleep(1)
##\brief Publishes messages for aggregator testing of expected items.

PKG = 'test_diagnostic_aggregator'

import rospy
from time import sleep

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus

if __name__ == '__main__':
    rospy.init_node('diag_pub')
    pub = rospy.Publisher('/diagnostics', DiagnosticArray)
        
    while not rospy.is_shutdown():
        array = DiagnosticArray()
        array.header.stamp = rospy.get_rostime()
        array.status = [
            # Match no analyze analyzer
            DiagnosticStatus(0, 'Match Item', 'OK', '', []),

            # Generic for something else
            DiagnosticStatus(1, 'Something', 'OK', '', []),
            DiagnosticStatus(2, 'Something Else', 'OK', '', []),
            
            # OtherAnalyzer for Other
            DiagnosticStatus(2, 'other1', 'OK', '', []),
            DiagnosticStatus(1, 'other2', 'OK', '', []),
            DiagnosticStatus(0, 'other3', 'OK', '', [])]
        
        pub.publish(array)
Exemple #58
0
        #S = tok[0][1]
        #all is good if we are synchronizing to a source
        if tok[0][1] == '*':
          status.level = 0
        #print M, S
    
  except subprocess.CalledProcessError as e:
    rospy.logerr(e.output)
    status.values.append(KeyValue(key=e.output, value=chr(2)))


if __name__ == '__main__':
  hostname = socket.gethostname()
  rospy.init_node('chronyStatus_'+hostname)
  pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1, latch=True)
  array = DiagnosticArray()
  status = DiagnosticStatus(name='ChronyStatus',\
                            level=0,\
                            hardware_id=hostname)

  array.status = [status]
  rate = rospy.Rate(0.2) # query and publish chrony information once every 5 seconds
  
  chronyVersion = checkChronyVersion()
  #chronyMinVersion = 1.29
  
  #publish error and exit if chronyMinVersion is not satisfied
  #if chronyVersion < chronyMinVersion:
  #  rospy.logerr('ChronyStatus requires chrony version ' + str(chronyMinVersion) + \
  #               ' or greater, version ' + str(chronyVersion) + ' detected, exiting')
import roslib; roslib.load_manifest(PKG)


import rospy
from time import sleep

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus

if __name__ == '__main__':
    rospy.init_node('diag_pub')
    pub = rospy.Publisher('/diagnostics', DiagnosticArray)
    
    start_time = rospy.get_time()
    
    while not rospy.is_shutdown():
        array = DiagnosticArray()
        array.status = [
            # GenericAnalyzer my_path
            DiagnosticStatus(0, 'expected1', 'OK', '', []),
            DiagnosticStatus(1, 'expected2', 'OK', '', []),
            DiagnosticStatus(2, 'expected3', 'OK', '', []),
            
            DiagnosticStatus(0, 'startswith1', 'OK', '', []),
            DiagnosticStatus(0, 'startswith2', 'OK', '', []),
            DiagnosticStatus(1, 'startswith3', 'OK', '', []),
            
            # OtherAnalyzer for Other
            DiagnosticStatus(2, 'other2', 'OK', '', []),
            DiagnosticStatus(0, 'other3', 'OK', '', [])]
        
        array.header.stamp = rospy.get_rostime()