Пример #1
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)
Пример #2
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)
Пример #3
0
    def update_diagnostics(self, event=None):
        stat = DiagnosticStatus()
        stat.level = DiagnosticStatus.WARN
        stat.name = '%s NTP Offset' % self.diag_hostname
        stat.message = 'No Data'
        stat.hardware_id = self.diag_hostname
        stat.values = []

        try:
            for st, host, off in [(stat, self.ntp_server, self.offset)]:
                p = Popen(["ntpdate", "-q", host],
                          stdout=PIPE,
                          stdin=PIPE,
                          stderr=PIPE)
                retcode = p.wait()
                stdout, stderr = p.communicate()
                try:
                    stdout = stdout.decode()  #python3
                except (UnicodeDecodeError, AttributeError):
                    pass

                if retcode != 0:
                    st.level = DiagnosticStatus.ERROR
                    st.message = 'ntpdate Error'
                    st.values = [
                        KeyValue(key='ntpdate Error', value=stderr),
                        KeyValue(key='Output', value=stdout)
                    ]
                    continue

                measured_offset = float(
                    re.search("offset (.*),", stdout).group(1)) * 1000000

                st.level = DiagnosticStatus.OK
                st.message = "OK"
                st.values = [
                    KeyValue("NTP Server", self.ntp_server),
                    KeyValue("Offset (us)", str(measured_offset)),
                    KeyValue("Offset tolerance (us)", str(off)),
                    KeyValue("Offset tolerance (us) for Error",
                             str(self.error_offset))
                ]

                if (abs(measured_offset) > off):
                    st.level = DiagnosticStatus.WARN
                    st.message = "NTP Offset Too High"
                if (abs(measured_offset) > self.error_offset):
                    st.level = DiagnosticStatus.ERROR
                    st.message = "NTP Offset Too High"

        except Exception as e:
            stat.level = DiagnosticStatus.ERROR
            stat.message = 'ntpdate Exception'
            stat.values = [KeyValue(key='Exception', value=str(e))]

        self.msg = DiagnosticArray()
        self.msg.header.stamp = rospy.get_rostime()
        self.msg.status = [stat]
Пример #4
0
def ntp_monitor(ntp_hostname, offset=500, self_offset=500):
    try:
        offset = int(offset)
        self_offset = int(self_offset)
    except:
        parser.error("offset must be a number")

    pub = rospy.Publisher("/diagnostics", DiagnosticArray)
    rospy.init_node(NAME, anonymous=True)

    hostname = socket.gethostname()

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

    self_stat = DiagnosticStatus()
    self_stat.level = 0
    self_stat.name = "NTP offset from: "+ hostname + " to: self."
    self_stat.message = "Acceptable synchronization"
    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 = 0
                st.message = "Acceptable synchronization"
                st.values = [ KeyValue("offset (us)", str(measured_offset)) ]
            
                if (abs(measured_offset) > off):
                    st.level = 2
                    st.message = "Offset too great"
                                
            else:
                st.level = 2
                st.message = "Error running ntpupdate"
                st.values = [ KeyValue("offset (us)", "N/A") ]


        pub.publish(DiagnosticArray(None, [stat, self_stat]))
        time.sleep(1)
Пример #5
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)
Пример #6
0
 def battery_status(self):
     stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("Voltage avg(Vin)", str(self.robot.get_avg_voltageIn())),
         KeyValue("Voltage max(Vin)", str(self.robot.get_max_voltageIn())),
         KeyValue("Voltage min(Vin)", str(self.robot.get_min_voltageIn())),
         KeyValue("Current avg(A)", str(self.robot.get_avg_current())),
         KeyValue("Current max(A)", str(self.robot.get_avg_current())),
         KeyValue("Current min(A)", str(self.robot.get_avg_current())),
         KeyValue("Voltage avg(V5V)", str(self.robot.get_avg_5voltage())),
         KeyValue("Voltage max(V5V)", str(self.robot.get_max_5voltage())),
         KeyValue("Voltage min(V5V)", str(self.robot.get_min_5voltage())),
         KeyValue("Voltage avg(V3.3)", str(self.robot.get_avg_3voltage())),
         KeyValue("Voltage max(V3.3)", str(self.robot.get_max_3voltage())),
         KeyValue("Voltage min(V3.3)", str(self.robot.get_min_3voltage()))]
     if self.robot.get_status().VoltageLow == True:
         stat.level = DiagnosticStatus.WARN
         stat.message = "Voltage too low"
     if self.robot.get_status().CurrentError == True:
         stat.level = DiagnosticStatus.WARN
         stat.message = "Current error"
     if self.robot.get_status().Voltage3v3Low == True:
         stat.level = DiagnosticStatus.WARN
         stat.message = "Voltage3.3 too low"
     return stat
def _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
Пример #8
0
 def fill_diags(name, summary, hid, diags):
     ds = DiagnosticStatus()
     ds.values = [KeyValue(k, str(v)) for (k, v) in diags]
     ds.hardware_id = hid
     ds.name = rospy.get_caller_id().lstrip('/') + ": " + name
     ds.message = summary
     return ds
Пример #9
0
 def info_status(self):
     stat = DiagnosticStatus(name="Info_Platform", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("", str(self.robot.get_status()[0])),
         KeyValue("", str(self.robot.get_status()[1])),
         KeyValue("", str(self.robot.get_status()[2]))]
     return stat
Пример #10
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)
Пример #11
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)
Пример #12
0
 def speed_status(self):
     stat = DiagnosticStatus(name="Speed", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("Linear speed (Vx)", str(self.robot.get_movesteer(None)[0])),
         KeyValue("Angular speed (Vth)", str(self.robot.get_movesteer(None)[2]))]
     if self.robot.get_movesteer(None)[0] > self.speedLimit:
         stat.level = DiagnosticStatus.WARN
         stat.message = "speed is too high"
     return stat
Пример #13
0
 def connection_status(self):
     stat = DiagnosticStatus(name="Connection", level=DiagnosticStatus.OK, message="OK")
     stat.values = [
         KeyValue("Baudrate", str(self.robot.get_connection_info()["baudrate"])),
         KeyValue("Comport", str(self.robot.get_connection_info()["comport"]))]
     if self.robot.is_connected() == False:
         stat.message = "disconnected"
         stat.level = DiagnosticStatus.ERROR
     return stat
Пример #14
0
 def cb_diagnostics_update(self):
     tabpage_status = DiagnosticStatus()
     tabpage_status.level = tabpage_status.OK
     tabpage_status.name = self.handler_name + " Handler"
     tabpage_status.hardware_id = self.ros_name
     tabpage_status.message = "OK"
     tabpage_status.values = []
     tabpage_status.values.append(KeyValue(key="Number of Clients",
                                           value=str(len(self.osc_clients))))
     return tabpage_status
Пример #15
0
def ntp_monitor(ntp_hostname,
                offset=500,
                self_offset=500,
                diag_hostname=None,
                error_offset=5000000,
                do_self_test=True):
    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 = DiagnosticStatus.OK
    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():
        msg = DiagnosticArray()
        msg.header.stamp = rospy.get_rostime()

        st = ntp_diag(stat, ntp_hostname, offset, error_offset)
        if st is not None:
            msg.status.append(st)

        if do_self_test:
            st = ntp_diag(self_stat, hostname, self_offset, error_offset)
            if st is not None:
                msg.status.append(st)

        pub.publish(msg)
        time.sleep(1)
    def get_diagnostics_message(self):
        message = DiagnosticStatus()
        message.name = "ODrive report"
        message.message = "ODrive diagnostics"
        message.hardware_id = self.serial_no
        if self.device is None:
            message.level = 1
            message.values = [KeyValue("connected", str(False))]
        else:
            message.values = [
                KeyValue("connected", str(True)),
                KeyValue("bus-voltage", str(self.device.vbus_voltage)),
                KeyValue("axis0-error",
                         oenums.errors.axis[self.device.axis0.error]),
                KeyValue(
                    "axis0-encoder-error",
                    oenums.errors.encoder[self.device.axis0.encoder.error]),
                KeyValue("axis0-motor-error",
                         oenums.errors.motor[self.device.axis0.motor.error]),
                # KeyValue("axis0-controller-error", oenums.errors.controller[self.device.axis0.controller.error]),
                KeyValue(
                    "axis0-ctrl-mode", oenums.control_modes[
                        self.device.axis0.controller.config.control_mode]),
                KeyValue("axis0-state",
                         oenums.axis_states[self.device.axis0.current_state]),
                KeyValue("axis1-error",
                         oenums.errors.axis[self.device.axis1.error]),
                KeyValue(
                    "axis1-encoder-error",
                    oenums.errors.encoder[self.device.axis1.encoder.error]),
                KeyValue("axis1-motor-error",
                         oenums.errors.motor[self.device.axis1.motor.error]),
                # KeyValue("axis1-controller-error", oenums.errors.controller[self.device.axis1.controller.error]),
                KeyValue(
                    "axis1-ctrl-mode", oenums.control_modes[
                        self.device.axis1.controller.config.control_mode]),
                KeyValue("axis1-state",
                         oenums.axis_states[self.device.axis1.current_state]),
            ]

        # print(repr(message))
        return message
Пример #17
0
 def cb_diagnostics_update(self):
     tabpage_status = DiagnosticStatus()
     tabpage_status.level = tabpage_status.OK
     tabpage_status.name = self.handler_name + " Handler"
     tabpage_status.hardware_id = self.ros_name
     tabpage_status.message = "OK"
     tabpage_status.values = []
     tabpage_status.values.append(
         KeyValue(key="Number of Clients",
                  value=str(len(self.osc_clients))))
     return tabpage_status
    def publish_motor_status(self, event=None):
        status = self.motor.get_status()
        data_list = []

        for key in status:
            data_list.append(KeyValue(key, str(status[key])))

            msg = DiagnosticStatus()
            msg.values = data_list

            self.motor_status_pub.publish(msg)
Пример #19
0
    def get_diagnostic(self):
        diag_msg = DiagnosticStatus()

        sys_status, gyro_status, accel_status, mag_status = self.bno.get_calibration_status(
        )
        sys = KeyValue("System status", str(sys_status))
        gyro = KeyValue("Gyro calibration status", str(gyro_status))
        accel = KeyValue("Accelerometer calibration status", str(accel_status))
        mag = KeyValue("Magnetometer calibration status", str(mag_status))

        diag_msg.values = [sys, gyro, accel, mag]
        return diag_msg
Пример #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
def joint_to_diag(js):
    global has_warned_invalid
    ds = DiagnosticStatus()
    ds.level = DiagnosticStatus.OK
    ds.message = 'OK'

    # Hack to stop gripper joints from being "uncalibrated"
    if not js.is_calibrated and js.name.find("float") < 0 and js.name.find(
            "finger") < 0:
        ds.level = DiagnosticStatus.WARN
        ds.message = 'Uncalibrated'

    if check_nan and (math.isnan(js.position) or math.isnan(js.velocity)
                      or math.isnan(js.measured_effort)
                      or math.isnan(js.commanded_effort)):
        ds.level = DiagnosticStatus.ERROR
        ds.message = 'NaN in joint data'
        if not has_warned_invalid:
            rospy.logerr(
                "NaN value for joint data. controller_manager restart required."
            )
            has_warned_invalid = True

    if check_nan and (math.isinf(js.position) or math.isinf(js.velocity)
                      or math.isinf(js.measured_effort)
                      or math.isinf(js.commanded_effort)):
        ds.level = DiagnosticStatus.ERROR
        ds.message = 'Inf in joint data'
        if not has_warned_invalid:
            rospy.logerr(
                "Infinite value for joint data. controller_manager restart required."
            )
            has_warned_invalid = True

    ds.name = "Joint (%s)" % js.name
    ds.values = [
        KeyValue('Position', str(js.position)),
        KeyValue('Velocity', str(js.velocity)),
        KeyValue('Measured Effort', str(js.measured_effort)),
        KeyValue('Commanded Effort', str(js.commanded_effort)),
        KeyValue('Calibrated', str(js.is_calibrated)),
        KeyValue('Odometer', str(js.odometer)),
        KeyValue('Max Position', str(js.max_position)),
        KeyValue('Min Position', str(js.min_position)),
        KeyValue('Max Abs. Velocity', str(js.max_abs_velocity)),
        KeyValue('Max Abs. Effort', str(js.max_abs_effort)),
        KeyValue('Limits Hit', str(js.violated_limits))
    ]

    return ds
Пример #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)
 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)
Пример #25
0
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)
Пример #26
0
    def publish_status(self):
        stat = DiagnosticStatus()
        stat.name = self.name
        is_connect_ok = self.check_connection()

        if is_connect_ok:
            stat.hardware_id = str(self._info_data['serial'])
            model = str(self._info_data['model'])
            top_temp = self.convertTemp(
                self._diag_data['volt_temp']['top']['lm20_temp'])
            bot_temp = self.convertTemp(
                self._diag_data['volt_temp']['bot']['lm20_temp'])
            i_out = self.convertAmp(
                self._diag_data['volt_temp']['bot']['i_out'])
            v_in = self.convertVolt(
                self._diag_data['volt_temp']['bot']['pwr_v_in'])
            stat.values = [
                KeyValue(key='ConnectionStatus', value='OK'),
                KeyValue(key='Model', value=str(model)),
                KeyValue(key='TopTemp[DegC]', value=str(round(top_temp, 3))),
                KeyValue(key='BottomTemp[DegC]', value=str(round(bot_temp,
                                                                 3))),
                KeyValue(key='Iout[V]', value=str(round(i_out, 3))),
                KeyValue(key='Vin[V]', value=str(round(v_in, 3)))
            ]
            self.judge_risk_level(top_temp, bot_temp, i_out, v_in, stat)
        else:
            stat.level = DiagnosticStatus.ERROR
            stat.hardware_id = 'Velodyne'
            stat.message = 'Connection Lost ' + self._ip
            stat.values = [KeyValue(key='ConnectionStatus', value='N/A')]

        msg = DiagnosticArray()
        msg.header.stamp = rospy.get_rostime()
        msg.status.append(stat)
        self._pub.publish(msg)
    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)
Пример #28
0
 def cb_diagnostics_update(self):
     """
     Callback periodically called to update the diagnostics status of the
     tabpage handler.
     
     @return: A status message for the tabpage handler
     @rtype: C{diagnostic_msgs/DiagnosticStatus}
     """
     tabpage_status = DiagnosticStatus()
     tabpage_status.level = tabpage_status.OK
     tabpage_status.name = " ".join([self.handler_name,
                                     "Handler"])
     tabpage_status.hardware_id = self.parent.ros_name
     tabpage_status.message = "OK"
     tabpage_status.values = []
     return tabpage_status
Пример #29
0
def joint_to_diag(js):
    d = DiagnosticStatus()
    d.level = 0
    d.message = ''
    if not js.is_calibrated:
        d.level = 1
        d.message = 'UNCALIBRATED'
    d.name = "Joint (%s)" % js.name
    d.values = [
        KeyValue('Position', str(js.position)),
        KeyValue('Velocity', str(js.velocity)),
        KeyValue('Applied Effort', str(js.applied_effort)),
        KeyValue('Commanded Effort', str(js.commanded_effort)),
        KeyValue('Calibrated', str(js.is_calibrated))
    ]
    return d
    def publish(self, msg):
        """Publish a single diagnostic status or a vector of diagnostic statuses."""
        if not type(msg) is list:
            msg = [msg]

        for stat in msg:
            stat.name = self.node.get_name() + ': ' + stat.name
        now = self.clock.now()

        da = DiagnosticArray()
        db = DiagnosticStatus()
        db.name = stat.name
        db.message = stat.message
        db.hardware_id = stat.hardware_id
        db.values = stat.values
        da.status.append(db)
        da.header.stamp = now.to_msg()  # Add timestamp for ROS 0.10
        self.publisher.publish(da)
Пример #31
0
    def reset(self):
        """Resets the environment to an initial state and returns an initial observation.

        Note that this function should not reset the environment's random
        number generator(s); random variables in the environment's state should
        be sampled independently between multiple calls to `reset()`. In other
        words, each call of `reset()` should yield an environment suitable for
        a new episode, independent of previous episodes.
        Returns:
            observation (object): the initial observation.
        """
        if self._has_state():
            # generate log
            info = {
                'episode': self.__episode,
                'net_reward': self.__net_reward,
                'duration': (rospy.Time.now() - self.__start_time).to_sec()
            }
            info.update(self._get_state()[3])
            # send message
            msg = DiagnosticStatus()
            msg.level = DiagnosticStatus.OK
            msg.name = 'ROS-Gym Interface'
            msg.message = 'log of episode data'
            msg.hardware_id = self.__LOG_ID
            msg.values = [
                KeyValue(key=key, value=str(value))
                for key, value in info.items()
            ]
            self.__log_pub.publish(msg)
            # update variables (update time after reset)
            self.__episode += 1
            self.__net_reward = 0

        # reset
        if not self.__EVAL_MODE:
            self._reset_env()
        self._reset_self()
        self.__step_time_and_wait_for_state(5)
        self.__start_time = rospy.Time.now()  # logging
        return self._get_state()[0]
    def check_ok(self):
        with self._mutex:
            stat = 0 if self._ok else 2
            msg = '' if self._ok else 'Dropped Packets'

            if rospy.get_time() - self._update_time > 3:
                stat = 3
                msg = 'Packet Data Stale'
                if self._update_time == -1:
                    msg = 'No Packet Data'
        
            diag = DiagnosticStatus()
            diag.name = 'EC Stats Packet Data'
            diag.level = stat
            diag.message = msg
            if diag.level == 0:
                diag.message = 'OK'
            
            diag.values = [
                KeyValue(key='Has Link?',              value=str(self._has_link)),
                KeyValue(key='Dropped Since Reset',    value=str(self._total_dropped - self._drop_count_at_reset)),
                KeyValue(key='Total Drops',            value=str(self._total_dropped)),
                KeyValue(key='Lost Link Count',        value=str(self._lost_link_count)),
                KeyValue(key='Lost Links Since Reset', value=str(self._lost_link_count_since_reset)),
                KeyValue(key='Number of Resets',       value=str(self._reset_count)),
                KeyValue(key='Time Since Last Reset',  value=str(rospy.get_time() - self._time_at_last_reset)),
                KeyValue(key='Drops at Last Reset',    value=str(self._drop_count_at_reset)),
                KeyValue(key='Max Device Count',       value=str(self._max_device_count)),
                KeyValue(key='Interval Drops',         value=str(self._interval_dropped)),
                KeyValue(key='Total Late Packets',     value=str(self._total_late)),
                KeyValue(key='Interval Late Packets',  value=str(self._interval_late)),
                KeyValue(key='Total Sent Packets',     value=str(self._total_sent)),
                KeyValue(key='Interval Sent Packets',  value=str(self._interval_sent)),
                KeyValue(key='Total Bandwidth',        value=str(self._total_bandwidth)),
                KeyValue(key='Interval Bandwidth',     value=str(self._interval_bandwidth))
                ]
        
        return stat, msg, [ diag ]
Пример #33
0
    def __init__(self, argv=sys.argv):
        rospy.init_node("ntp_monitor")
        self.parse_args(argv)

        stat = DiagnosticStatus()
        stat.level = DiagnosticStatus.WARN
        stat.name = '%s NTP Offset' % self.diag_hostname
        stat.message = 'No Data'
        stat.hardware_id = self.diag_hostname
        stat.values = []
        self.msg = DiagnosticArray()
        self.msg.header.stamp = rospy.get_rostime()
        self.msg.status = [stat]

        self.update_diagnostics()

        self.diag_pub = rospy.Publisher("/diagnostics",
                                        DiagnosticArray,
                                        queue_size=1)
        self.diag_timer = rospy.Timer(rospy.Duration(1.0),
                                      self.publish_diagnostics)
        self.monitor_timer = rospy.Timer(rospy.Duration(60.0),
                                         self.update_diagnostics)
Пример #34
0
    def publish(self, sensor_state, gyro):
        curr_time = sensor_state.header.stamp
        # limit to 5hz
        if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
            return
        self.last_diagnostics_time = curr_time

        diag = DiagnosticArray()
        diag.header.stamp = curr_time
        stat = DiagnosticStatus()

        #node status
        stat = DiagnosticStatus(name="TurtleBot Node",
                                level=DiagnosticStatus.OK,
                                message="RUNNING")
        diag.status.append(stat)

        #mode info
        stat = DiagnosticStatus(name="Operating Mode",
                                level=DiagnosticStatus.OK)
        try:
            stat.message = self.oi_mode[sensor_state.oi_mode]
        except KeyError as ex:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Invalid OI Mode Reported %s" % ex
            rospy.logwarn(stat.message)
        diag.status.append(stat)

        #battery info
        stat = DiagnosticStatus(name="Battery",
                                level=DiagnosticStatus.OK,
                                message="OK")
        values = stat.values
        if sensor_state.charging_state == 5:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Charging Fault Condition"
            values.append(
                KeyValue("Charging State",
                         self.charging_state[sensor_state.charging_state]))
        values.extend([
            KeyValue("Voltage (V)", str(sensor_state.voltage / 1000.0)),
            KeyValue("Current (A)", str(sensor_state.current / 1000.0)),
            KeyValue("Temperature (C)", str(sensor_state.temperature)),
            KeyValue("Charge (Ah)", str(sensor_state.charge / 1000.0)),
            KeyValue("Capacity (Ah)", str(sensor_state.capacity / 1000.0))
        ])
        diag.status.append(stat)

        #charging source
        stat = DiagnosticStatus(name="Charging Sources",
                                level=DiagnosticStatus.OK)
        try:
            stat.message = self.charging_source[
                sensor_state.charging_sources_available]
        except KeyError as ex:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Invalid Charging Source %s, actual value: %i" % (
                ex, sensor_state.charging_sources_available)
            rospy.logwarn(stat.message)
        diag.status.append(stat)
        #cliff sensors
        stat = DiagnosticStatus(name="Cliff Sensor",
                                level=DiagnosticStatus.OK,
                                message="OK")
        if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
            stat.level = DiagnosticStatus.WARN
            if (sensor_state.current / 1000.0) > 0:
                stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
                stat.level = DiagnosticStatus.OK  # We're OK here
            else:
                stat.message = "Near Cliff"
        stat.values = [
            KeyValue("Left", str(sensor_state.cliff_left)),
            KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
            KeyValue("Front Left", str(sensor_state.cliff_front_left)),
            KeyValue("Front Left Signal",
                     str(sensor_state.cliff_front_left_signal)),
            KeyValue("Front Right", str(sensor_state.cliff_right)),
            KeyValue("Front Right Signal",
                     str(sensor_state.cliff_right_signal)),
            KeyValue("Right", str(sensor_state.cliff_front_right)),
            KeyValue("Right Signal",
                     str(sensor_state.cliff_front_right_signal))
        ]
        diag.status.append(stat)
        #Wall sensors
        stat = DiagnosticStatus(name="Wall Sensor",
                                level=DiagnosticStatus.OK,
                                message="OK")
        #wall always seems to be false???
        if sensor_state.wall:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Near Wall"
        stat.values = [
            KeyValue("Wall", str(sensor_state.wall)),
            KeyValue("Wall Signal", str(sensor_state.wall_signal)),
            KeyValue("Virtual Wall", str(sensor_state.virtual_wall))
        ]
        diag.status.append(stat)
        #Gyro
        stat = DiagnosticStatus(name="Gyro Sensor",
                                level=DiagnosticStatus.OK,
                                message="OK")
        if gyro is None:
            stat.level = DiagnosticStatus.WARN
            stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node."
        elif gyro.cal_offset < 100:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
        elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."

        if gyro is not None:
            stat.values = [
                KeyValue("Gyro Enabled", str(gyro is not None)),
                KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
                KeyValue("Calibration Offset", str(gyro.cal_offset)),
                KeyValue("Calibration Buffer", str(gyro.cal_buffer))
            ]
        diag.status.append(stat)
        #Digital IO
        stat = DiagnosticStatus(name="Digital Outputs",
                                level=DiagnosticStatus.OK,
                                message="OK")
        out_byte = sensor_state.user_digital_outputs
        stat.values = [
            KeyValue("Raw Byte", str(out_byte)),
            KeyValue("Digital Out 2", self.digital_outputs[out_byte % 2]),
            KeyValue("Digital Out 1",
                     self.digital_outputs[(out_byte >> 1) % 2]),
            KeyValue("Digital Out 0",
                     self.digital_outputs[(out_byte >> 2) % 2])
        ]
        diag.status.append(stat)
        #publish
        self.diag_pub.publish(diag)
Пример #35
0
    def __init__(self):
        rospy.init_node('system_node', log_level=rospy.DEBUG)

        # self.load_publisher = rospy.Publisher('~load', msgs.CPUUsage, queue_size=1)
        # self.cpu_publisher = rospy.Publisher('~cpu', msgs.CPUUsage, queue_size=1)
        self.memory_publisher = rospy.Publisher('~memory',
                                                msgs.MemoryUsage,
                                                queue_size=1)
        self.disk_publisher = rospy.Publisher('~disk',
                                              msgs.DiskUsage,
                                              queue_size=1)
        self.diagnostics_pub = rospy.Publisher('/diagnostics',
                                               DiagnosticArray,
                                               queue_size=10)

        self.debug_level_sub = rospy.Subscriber('~debug_level', Int16,
                                                self.on_debug_level)

        self.debug_level = ONLY_ERRORS

        self.drive_path = '/dev/root'

        self.rate = 1  #float(rospy.get_param("~rate", 1)) # hertz

        rospy.loginfo('Ready')

        r = rospy.Rate(self.rate)
        while not rospy.is_shutdown():

            # Find normalized system load.
            # The ideal normalized system load less than or equal 1.0, meaning the processor is getting no more than the maximum number of tasks it can handle.
            proc_count = multiprocessing.cpu_count()
            load_1, load_5, load_15 = os.getloadavg()
            normalized_load_1 = load_1 / float(proc_count)
            normalized_load_5 = load_5 / float(proc_count)
            normalized_load_15 = load_15 / float(proc_count)
            normalized_load_level = OK
            if normalized_load_level >= 4.0:
                normalized_load_level = ERROR
            elif normalized_load_level >= 2.0:
                normalized_load_level = WARN

            # Find CPU.
            cpu_usage_percent = to_percent(
                getoutput(
                    "grep 'cpu ' /proc/stat | awk '{usage=($2+$4)*100/($2+$4+$5)} END {print usage }'"
                ))
            cpu_usage_percent_level = OK
            if cpu_usage_percent >= c.CPU_USAGE_PERCENT_ERROR:
                cpu_usage_percent_level = ERROR
            elif cpu_usage_percent >= c.CPU_USAGE_PERCENT_WARN:
                cpu_usage_percent_level = WARN

            # msg = msgs.CPUUsage()
            # msg.header.stamp = rospy.Time.now()
            # msg.percent_used = cpu_usage_percent
            # self.cpu_publisher.publish(msg)

            # Find memory.
            memory_usage_free_gbytes = to_float(
                getoutput("free -m|grep -i 'Mem:'|awk '{print $4}'"))
            memory_usage_used_gbytes = to_float(
                getoutput("free -m|grep -i 'Mem:'|awk '{print $3}'"))
            memory_usage_total_gbytes = to_float(
                getoutput("free -m|grep -i 'Mem:'|awk '{print $2}'"))
            memory_usage_percent = -1
            if memory_usage_used_gbytes != -1 and memory_usage_total_gbytes != -1:
                memory_usage_percent = memory_usage_used_gbytes / memory_usage_total_gbytes * 100
            memory_usage_percent_level = OK
            if memory_usage_percent >= c.MEMORY_USAGE_PERCENT_ERROR:
                memory_usage_percent_level = ERROR
            elif memory_usage_percent >= c.MEMORY_USAGE_PERCENT_WARN:
                memory_usage_percent_level = WARN

            msg = msgs.MemoryUsage()
            msg.header.stamp = rospy.Time.now()
            msg.used_gbytes = memory_usage_used_gbytes
            msg.free_gbytes = memory_usage_free_gbytes
            msg.total_gbytes = memory_usage_total_gbytes
            msg.percent_used = memory_usage_percent
            self.memory_publisher.publish(msg)

            # Find disk.
            disk_usage_percent = to_percent(
                getoutput("df -H | grep -i " + self.drive_path +
                          " | awk '{print $5}'"))
            disk_usage_free_gbytes = to_gbytes(
                getoutput("df -H | grep -i " + self.drive_path +
                          " | awk '{print $4}'"))
            disk_usage_used_gbytes = to_gbytes(
                getoutput("df -H | grep -i " + self.drive_path +
                          " | awk '{print $3}'"))
            disk_usage_total_gbytes = to_gbytes(
                getoutput("df -H | grep -i " + self.drive_path +
                          " | awk '{print $2}'"))
            disk_usage_level = OK
            if disk_usage_percent >= c.DISK_USAGE_PERCENT_ERROR:
                disk_usage_level = ERROR
            if disk_usage_percent >= c.DISK_USAGE_PERCENT_WARN:
                disk_usage_level = WARN

            msg = msgs.DiskUsage()
            msg.header.stamp = rospy.Time.now()
            msg.used_gbytes = disk_usage_used_gbytes
            msg.free_gbytes = disk_usage_free_gbytes
            msg.total_gbytes = disk_usage_total_gbytes
            msg.percent_used = disk_usage_percent
            self.disk_publisher.publish(msg)

            # Find CPU clock speed.
            min_ghz = get_cpu_clock_speed_min() / 1000. / 1000.
            max_ghz = get_cpu_clock_speed_max() / 1000. / 1000.
            cpu_clock_speed = get_cpu_clock_speed()
            #             print('cpu_clock_speed:', cpu_clock_speed)
            cpu_clock_speed_percent = get_cpu_clock_speed_percent()
            #             print('cpu_clock_speed_percent:', cpu_clock_speed_percent)
            cpu_clock_speed_percent_level = OK
            # if cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_ERROR:
            # cpu_clock_speed_percent_level = ERROR
            # elif cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_WARN:
            # cpu_clock_speed_percent_level = WARN

            # Find CPU temperature.
            cpu_temp = get_cpu_temp()
            cpu_temp_level = OK
            if cpu_temp >= c.CPU_TEMP_ERROR:
                cpu_temp_level = ERROR
            elif cpu_temp >= c.CPU_TEMP_WARN:
                cpu_temp_level = WARN

            # Publish standard diagnostics.
            array = DiagnosticArray()
            statuses = []

            normalized_load_status = DiagnosticStatus(
                name='Normalized System Load',
                level=normalized_load_level,
                message=str(normalized_load_15))
            normalized_load_status.values = [
                KeyValue(key='normalized load 1',
                         value=str(normalized_load_1)),
                KeyValue(key='normalized load 5',
                         value=str(normalized_load_5)),
                KeyValue(key='normalized load 15',
                         value=str(normalized_load_15)),
            ]
            if self.debug_level >= UP_TO_OK or normalized_load_level != OK:
                statuses.append(normalized_load_status)

            cpu_temperature_status = DiagnosticStatus(name='CPU Temperature',
                                                      level=cpu_temp_level,
                                                      message=str(cpu_temp))
            cpu_temperature_status.values = [
                KeyValue(key='celcius', value=str(cpu_temp)),
            ]
            if self.debug_level >= UP_TO_OK or cpu_temp >= c.CPU_TEMP_ERROR:
                statuses.append(cpu_temperature_status)

            cpu_usage_status = DiagnosticStatus(name='CPU Usage',
                                                level=cpu_usage_percent_level,
                                                message=str(cpu_usage_percent))
            cpu_usage_status.values = [
                KeyValue(key='percent', value=str(cpu_usage_percent)),
            ]
            if self.debug_level >= UP_TO_OK or cpu_usage_percent_level != OK:
                statuses.append(cpu_usage_status)

            cpu_clock_speed_status = DiagnosticStatus(
                name='CPU Speed',
                level=cpu_clock_speed_percent_level,
                message=str(cpu_clock_speed_percent))
            cpu_clock_speed_status.values = [
                KeyValue(key='clock speed (GHz)', value=str(cpu_clock_speed)),
                KeyValue(key='min clock speed (GHz)', value=str(min_ghz)),
                KeyValue(key='max clock speed (GHz)', value=str(max_ghz)),
                KeyValue(key='clock speed (percent)',
                         value=str(cpu_clock_speed_percent)),
            ]
            if self.debug_level >= UP_TO_OK or cpu_clock_speed_percent_level != OK:
                statuses.append(cpu_clock_speed_status)

            disk_usage_status = DiagnosticStatus(
                name='Disk Usage',
                level=disk_usage_level,
                message=str(disk_usage_percent))
            disk_usage_status.values = [
                KeyValue(key='percent', value=str(disk_usage_percent)),
                KeyValue(key='free gb', value=str(disk_usage_free_gbytes)),
                KeyValue(key='used gb', value=str(disk_usage_used_gbytes)),
                KeyValue(key='total gb', value=str(disk_usage_total_gbytes)),
            ]
            if self.debug_level >= UP_TO_OK or disk_usage_level != OK:
                statuses.append(disk_usage_status)

            memory_usage_status = DiagnosticStatus(
                name='Memory Usage',
                level=memory_usage_percent_level,
                message=str(memory_usage_percent))
            memory_usage_status.values = [
                KeyValue(key='percent', value=str(memory_usage_percent)),
                KeyValue(key='free gb', value=str(memory_usage_free_gbytes)),
                KeyValue(key='used gb', value=str(memory_usage_used_gbytes)),
                KeyValue(key='total gb', value=str(memory_usage_total_gbytes)),
            ]
            if self.debug_level >= UP_TO_OK or memory_usage_percent_level != OK:
                statuses.append(memory_usage_status)

            if statuses:
                array.status = statuses
                # [
                # normalized_load_status,
                # cpu_temperature_status,
                # cpu_usage_status,
                # cpu_clock_speed_status,
                # disk_usage_status,
                # memory_usage_status,
                # ]
                self.diagnostics_pub.publish(array)

            r.sleep()
Пример #36
0
    def update(self, mech_state):
        self._rx_count += 1
        diag = DiagnosticStatus()
        diag.level = 0  # Default the level to 'OK'
        diag.name = "Trans. Monitor: %s" % self._joint
        diag.message = "OK"
        diag.values = []

        diag.values.append(KeyValue("Joint", self._joint))
        diag.values.append(KeyValue("Actuator", self._actuator))
        diag.values.append(KeyValue("Up position", str(self._up_ref)))
        diag.values.append(KeyValue("Down position", str(self._down_ref)))
        diag.values.append(KeyValue("Wrap", str(self._wrap)))
        diag.values.append(KeyValue("Max Limit", str(self._max)))
        diag.values.append(KeyValue("Min Limit", str(self._min)))

        diag.values.append(KeyValue("Deadband", str(self._deadband)))
        diag.values.append(KeyValue("Mech State RX Count",
                                    str(self._rx_count)))

        # Check if we can find both the joint and actuator
        act_names = [x.name for x in mech_state.actuator_states]
        act_exists = self._actuator in act_names
        act_exists_msg = 'Error'

        if act_exists:
            cal_reading = mech_state.actuator_states[act_names.index(
                self._actuator)].calibration_reading

        joint_names = [x.name for x in mech_state.joint_states]
        joint_exists = self._joint in joint_names
        jnt_exists_msg = 'Error'
        if joint_exists:
            position = mech_state.joint_states[joint_names.index(
                self._joint)].position
            calibrated = (mech_state.joint_states[joint_names.index(
                self._joint)].is_calibrated == 1)

        diag.values.append(KeyValue('Actuator Exists', str(act_exists)))
        diag.values.append(KeyValue('Joint Exists', str(joint_exists)))

        # First check existance of joint, actuator
        if not (act_exists and joint_exists):
            diag.level = 2
            diag.message = 'Actuators, Joints missing'
            self._ok = False
            return diag, False

        # Monitor current state
        reading_msg = 'OK'
        if not self.check_device(position, cal_reading, calibrated):
            self._ok = False
            self._num_errors += 1
            self._num_errors_since_reset += 1
            reading_msg = 'Broken'

        # If we've had an error since the last reset, we're no good
        if not self._ok:
            diag.message = 'Broken'
            diag.level = 2

        diag.values.insert(0, KeyValue('Transmission Status', diag.message))
        diag.values.insert(1, KeyValue('Current Reading', reading_msg))
        diag.values.append(KeyValue('Is Calibrated', str(calibrated)))
        diag.values.append(KeyValue('Calibration Reading', str(cal_reading)))
        diag.values.append(KeyValue('Joint Position', str(position)))

        diag.values.append(KeyValue('Total Errors', str(self._num_errors)))
        diag.values.append(
            KeyValue('Errors Since Reset', str(self._num_errors_since_reset)))

        self._max_position = max(self._max_position, position)
        diag.values.append(
            KeyValue('Max Obs. Position', str(self._max_position)))

        self._min_position = min(self._min_position, position)
        diag.values.append(
            KeyValue('Min Obs. Position', str(self._min_position)))

        return self._ok, diag
Пример #37
0
                          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')

    #else:
    while not rospy.is_shutdown():

        status.values = []
        status.values.append(
            KeyValue(key='chrony version', value=chronyVersion))

        getTracking(status)
        getSources(status)

        pub.publish(array)
        rate.sleep()
Пример #38
0
    def publish(self, sensor_state, gyro):
        curr_time = sensor_state.header.stamp
        # limit to 5hz
        if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
            return
        self.last_diagnostics_time = curr_time

        diag = DiagnosticArray()
        diag.header.stamp = curr_time
        stat = DiagnosticStatus()

        #node status
        stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.OK, message="RUNNING")
        diag.status.append(stat)

        #mode info
        stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK)
        try:
            stat.message = self.oi_mode[sensor_state.oi_mode]
        except KeyError as ex:
            stat.message = "Invalid OI Mode %s"%ex
            rospy.logwarn("Invalid OI Mode %s"%ex)
        diag.status.append(stat)
        
        #battery info
        stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
        values = stat.values
        if sensor_state.charging_state == 5:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Charging Fault Condition"
            values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state]))
        values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)),
                       KeyValue("Current (A)", str(sensor_state.current/1000.0)),
                       KeyValue("Temperature (C)",str(sensor_state.temperature)),
                       KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)),
                       KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))])
        diag.status.append(stat)
        
        #charging source
        stat = DiagnosticStatus(name="Charging Sources", level=DiagnosticStatus.OK)
        try:
            stat.message = self.charging_source[sensor_state.charging_sources_available]
        except KeyError as ex:
            stat.message = "Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available)
            rospy.logwarn("Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available))
        diag.status.append(stat)
        #cliff sensors
        stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
        if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
            stat.level = DiagnosticStatus.WARN
            if (sensor_state.current/1000.0)>0:
                stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
            else:
                stat.message = "Near Cliff"
        stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
                       KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
                       KeyValue("Front Left", str(sensor_state.cliff_front_left)),
                       KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
                       KeyValue("Front Right", str(sensor_state.cliff_right)),
                       KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
                       KeyValue("Right", str(sensor_state.cliff_front_right)),
                       KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
        diag.status.append(stat)
        #Wall sensors
        stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
        #wall always seems to be false??? 
        if sensor_state.wall:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Near Wall"
        stat.values = [KeyValue("Wall", str(sensor_state.wall)),
                       KeyValue("Wall Signal", str(sensor_state.wall_signal)),
                       KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
        diag.status.append(stat)
        #Gyro
        stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
        if gyro is None:
            stat.level = DiagnosticStatus.WARN
            stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node."   
        elif gyro.cal_offset < 100:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
        elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."

        if gyro is not None:    
            stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
                           KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
                           KeyValue("Calibration Offset", str(gyro.cal_offset)),
                           KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
        diag.status.append(stat)
        #Digital IO
        stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK")
        out_byte = sensor_state.user_digital_outputs
        stat.values = [KeyValue("Raw Byte", str(out_byte)),
                       KeyValue("Digital Out 2", self.digital_outputs[out_byte%2]),
                       KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]),
                       KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>2)%2])]
        diag.status.append(stat)
        #publish
        self.diag_pub.publish(diag)
Пример #39
0
    def publish(self, sensor_state):
        curr_time = sensor_state.header.stamp
        # limit to 5hz
        if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
            return
        self.last_diagnostics_time = curr_time

        diag = DiagnosticArray()
        diag.header.stamp = curr_time
        stat = DiagnosticStatus()

        #node status
        stat = DiagnosticStatus(name="Robo50 Node", level=DiagnosticStatus.OK, message="RUNNING")
        diag.status.append(stat)

        #battery info
#         stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
#         values = stat.values
#         if sensor_state.charging_state == 5:
#             stat.level = DiagnosticStatus.ERROR
#             stat.message = "Charging Fault Condition"
#             values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state]))
#         values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)),
#                        KeyValue("Current (A)", str(sensor_state.current/1000.0)),
#                        KeyValue("Temperature (C)",str(sensor_state.temperature)),
#                        KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)),
#                        KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))])
#         diag.status.append(stat)
        
#         #cliff sensors
#         stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
#         if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
#             stat.level = DiagnosticStatus.WARN
#             if (sensor_state.current/1000.0)>0:
#                 stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
#                 stat.level = DiagnosticStatus.OK # We're OK here
#             else:
#                 stat.message = "Near Cliff"
#         stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
#                        KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
#                        KeyValue("Front Left", str(sensor_state.cliff_front_left)),
#                        KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
#                        KeyValue("Front Right", str(sensor_state.cliff_right)),
#                        KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
#                        KeyValue("Right", str(sensor_state.cliff_front_right)),
#                        KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
#         diag.status.append(stat)
#         #Wall sensors
#         stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
#         #wall always seems to be false??? 
#         if sensor_state.wall:
#             stat.level = DiagnosticStatus.ERROR
#             stat.message = "Near Wall"
#         stat.values = [KeyValue("Wall", str(sensor_state.wall)),
#                        KeyValue("Wall Signal", str(sensor_state.wall_signal)),
#                        KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
#         diag.status.append(stat)

        #Gyro
#         stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
#         if gyro is None:
#             stat.level = DiagnosticStatus.WARN
#             stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the Robo50_node."   
#         elif gyro.cal_offset < 100:
#             stat.level = DiagnosticStatus.ERROR
#             stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/Robo50-bad-gyro-calibration-also."
#         elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
#             stat.level = DiagnosticStatus.ERROR
#             stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
# 
#         if gyro is not None:    
#             stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
#                            KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
#                            KeyValue("Calibration Offset", str(gyro.cal_offset)),
#                            KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
#         diag.status.append(stat)

        #IMU
        stat = DiagnosticStatus(name="IMU Sensor", level = DiagnosticStatus.OK, message = "OK")
        stat.values = [KeyValue("Compass Heading", str(sensor_state.imu.heading)),
                       KeyValue("Gyro", str(sensor_state.imu.gyro)),
                       KeyValue("Accelero", str(sensor_state.imu.accelero))]
        diag.status.append(stat)
        
        #Encoder
        stat = DiagnosticStatus(name="Encoder Counts", level = DiagnosticStatus.OK, message = "OK")
        stat.values = [KeyValue("Left Encoder", str(sensor_state.encoder_counts_left)),
                       KeyValue("Right Encoder", str(sensor_state.encoder_counts_right)),
                       KeyValue("Distance Left", str(sensor_state.distance_left)),
                       KeyValue("Distance Right", str(sensor_state.distance_right)),
                       KeyValue("Distance", str(sensor_state.distance)),
                       KeyValue("Angle", str(sensor_state.angle))]
        diag.status.append(stat)
        #publish
        self.diag_pub.publish(diag)
Пример #40
0
  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')

  #else:
  while not rospy.is_shutdown():

    status.values = []
    status.values.append(KeyValue(key='chrony version', value=chronyVersion) )

    getTracking(status)
    getSources(status)

    pub.publish(array)
    rate.sleep()
    pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)

    rospy.Subscriber("hw_monitor/diagnostics/motor_stall_l", Bool, motor_stall_l_callback)
    rospy.Subscriber("hw_monitor/diagnostics/motor_stall_r", Bool, motor_stall_r_callback)
    rospy.Subscriber("hw_monitor/diagnostics/batt_low", Bool, batt_low_callback)
    rospy.Subscriber("hw_monitor/diagnostics/batt_high", Bool, batt_high_callback)
    rospy.Subscriber("hw_monitor/diagnostics/controller", Bool, controller_callback)
    rospy.Subscriber("hw_monitor/diagnostics/aux_lights", Bool, aux_lights_callback)

    array = DiagnosticArray()

    my_rate = rospy.Rate(1.0)
    while not rospy.is_shutdown():

        motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Running")
        motor_l_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="False")]

        motor_r_stat = DiagnosticStatus(name="Motor Right", level=0, message="Running")
        motor_r_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="False")]

        if controller == True:
            motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Halted")
            motor_l_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="True")]
            motor_r_stat = DiagnosticStatus(name="Motor Right", level=0, message="Halted")
            motor_r_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="True")]

        if stall_l == True:
            motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Stalled")
            motor_l_stat.values = [KeyValue(key="Stall", value="True"), KeyValue(key="Controller Halt", value="False")]

        if stall_l == True & controller == True:
Пример #42
0
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from time import sleep


def callback(msg):
  print("Got msg")
  return EmptyResponse()

if __name__ == '__main__':
  rospy.init_node("test_node")
  
  rospy.Service('self_test', Empty, callback)
  
  pub = rospy.Publisher('/diagnostics', DiagnosticArray)
  
  # Publish diagnostics to check runtime monitor, rxconsole
  msg = DiagnosticArray()
  stat = DiagnosticStatus()
  stat.level = 0
  stat.name = 'Test Node'
  stat.message = 'OK'
  stat.hardware_id = 'HW ID'
  stat.values = [ KeyValue('Node Status', 'OK') ]
  msg.status.append(stat)

  while not rospy.is_shutdown():
    msg.header.stamp = rospy.get_rostime()
    pub.publish(msg)
    rospy.loginfo('Test node is printing things')
    sleep(1.0)
Пример #43
0
def ntp_monitor(namespace,
                offset=500,
                self_offset=500,
                diag_hostname=None,
                error_offset=5000000):
    rospy.init_node(NAME, anonymous=True)
    diag_updater = DiagnosticUpdater(
        name=namespace + 'ntp',
        display_name=diag_hostname + ' NTP',
    )

    hostname = socket.gethostname()
    if diag_hostname is None:
        diag_hostname = hostname

    ntp_hostname = rospy.get_param('~reference_host', 'ntp.ubuntu.com')
    offset = rospy.get_param('~offset_tolerance', 500)
    error_offset = rospy.get_param('~error_offset_tolerance', 5000000)

    stat = DiagnosticStatus()
    stat.level = 0
    stat.name = "NTP Offset"
    stat.message = "OK"
    stat.hardware_id = hostname
    stat.values = []
    stat_diagnostic = GenericDiagnostic('/offset')
    stat_diagnostic.add_to_updater(diag_updater)

    #    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)]:
            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:
                # Warning (not error), since this is a non-critical failure.
                st.level = DiagnosticStatus.WARN
                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)
                ]

        # Convert from ROS diagnostics to mbot_diagnostics for publishing.
        stat_diagnostic.set_status(Status(stat.level), stat.message)
        for diag_val in stat.values:
            stat_diagnostic.set_metric(diag_val.key, diag_val.value)

        time.sleep(1)
Пример #44
0
    def cb_diagnostics_update(self):
        """
        Callback periodically called to update the diagnostics status of the 
        TouchOscInterface.
        """
        msg = DiagnosticArray()
        msg.header.stamp = rospy.Time.now()
        msg.status = []

        # If the callbacks DiagnosticStatus message hasn't been populated,
        # do that now.
        if not self._diagnostic_status_callbacks:
            callback_status = DiagnosticStatus()
            callback_status.level = callback_status.OK
            callback_status.name = " ".join([self.ros_name,
                                             "Registered Callbacks"])
            callback_status.hardware_id = self.ros_name
            callback_status.message = "OK"
            callback_status.values = []
            diags = [(k, v) for k, v in walk_node(self._osc_receiver)]
            rospy.logdebug("Registered Callbacks:")
            for (k, v) in diags:
                rospy.logdebug('{0:<30}{1:<30}'.format(k, v))
                callback_status.values.append(KeyValue(key=k, value=v))
            self._diagnostic_status_callbacks = callback_status
        msg.status.append(self._diagnostic_status_callbacks)

        # Populate the clients DiagnosticStatus message
        diagnostic_status_clients = DiagnosticStatus()
        diagnostic_status_clients.level = diagnostic_status_clients.OK
        diagnostic_status_clients.name = " ".join([self.ros_name,
                                                    "Client Status"])
        diagnostic_status_clients.hardware_id = self.ros_name
        diagnostic_status_clients.message = "Listening on %d" % self.osc_port
        diagnostic_status_clients.values = []
        #TODO: The clients property accessor is not thread-safe, as far as I can
        #tell, but using a lock tends to make bonjour hang.
        clients = self.clients
        for client in clients.itervalues():
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address,
                                    value=client.servicename.split("[")[0]))
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address + " Type",
                                    value=client.client_type))
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address + " Current",
                                    value=client.active_tabpage))
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address + " Tabpages",
                                    value=", ".join(client.tabpages)))
        if len(self.clients) == 0:
            diagnostic_status_clients.message = "No clients detected"
        msg.status.append(diagnostic_status_clients)

        # For each registered tabpage handler, get a DiagnosticStatus message.
        for tabpage in self.tabpage_handlers.itervalues():
            msg.status.append(tabpage.cb_diagnostics_update())

        # Publish
        self.diagnostics_pub.publish(msg)
        reactor.callLater(1.0, self.cb_diagnostics_update)
Пример #45
0
    def publish_diagnostics_info(self):
        """
        Publishes at a rate of 1Hz ( because thats the rate real kobuki does it )
        For this first version we only publish battery status.
        :return:
        """
        battery_charge = self.get_simulated_battery_status()

        msg = DiagnosticArray()

        info_msg = DiagnosticStatus()

        header = Header()
        header.frame_id = ""
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.status.append(info_msg)

        values = []
        percent_value = KeyValue()
        percent_value.key = "Percent"
        percent_value.value = str(battery_charge)
        # TODO: Add all the other values
        voltage_value = KeyValue()
        voltage_value.key = "Voltage (V)"

        charge_value = KeyValue()
        charge_value.key = "Charge (Ah)"

        capacity_value = KeyValue()
        capacity_value.key = "Capacity (Ah)"

        source_value = KeyValue()
        source_value.key = "Source"

        charging_state_value = KeyValue()
        charging_state_value.key = "Charging State"

        current_value = KeyValue()
        current_value.key = "Current (A)"

        if battery_charge <= 10.0:
            level = DiagnosticStatus.ERROR
            message = "Empty Battery"

        elif 10.0 < battery_charge <= 20.0:
            level = DiagnosticStatus.WARN
            message = "Warning Low Battery"
        elif battery_charge >= 100.0:
            level = DiagnosticStatus.OK
            message = "Maximum"
        else:
            level = DiagnosticStatus.OK
            message = "NormalLevel"

        info_msg.name = "mobile_base_nodelet_manager: Battery"
        info_msg.hardware_id = "Kobuki"
        info_msg.message = message
        info_msg.level = level
        values.append(percent_value)
        values.append(voltage_value)
        values.append(charge_value)
        values.append(capacity_value)
        values.append(source_value)
        values.append(charging_state_value)
        values.append(current_value)

        info_msg.values = values

        msg.status.append(info_msg)

        self._pub.publish(msg)
Пример #46
0
    def cb_diagnostics_update(self):
        """
        Callback periodically called to update the diagnostics status of the 
        TouchOscInterface.
        """
        msg = DiagnosticArray()
        msg.header.stamp = rospy.Time.now()
        msg.status = []

        # If the callbacks DiagnosticStatus message hasn't been populated,
        # do that now.
        if not self._diagnostic_status_callbacks:
            callback_status = DiagnosticStatus()
            callback_status.level = callback_status.OK
            callback_status.name = " ".join([self.ros_name,
                                             "Registered Callbacks"])
            callback_status.hardware_id = self.ros_name
            callback_status.message = "OK"
            callback_status.values = []
            diags = [(k, v) for k, v in walk_node(self._osc_receiver)]
            rospy.logdebug("Registered Callbacks:")
            for (k, v) in diags:
                rospy.logdebug('{0:<30}{1:<30}'.format(k, v))
                callback_status.values.append(KeyValue(key=k, value=v))
            self._diagnostic_status_callbacks = callback_status
        msg.status.append(self._diagnostic_status_callbacks)

        # Populate the clients DiagnosticStatus message
        diagnostic_status_clients = DiagnosticStatus()
        diagnostic_status_clients.level = diagnostic_status_clients.OK
        diagnostic_status_clients.name = " ".join([self.ros_name,
                                                    "Client Status"])
        diagnostic_status_clients.hardware_id = self.ros_name
        diagnostic_status_clients.message = "Listening on %d" % self.osc_port
        diagnostic_status_clients.values = []
        #TODO: The clients property accessor is not thread-safe, as far as I can
        #tell, but using a lock tends to make bonjour hang.
        clients = self.clients
        for client in clients.itervalues():
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address,
                                    value=client.servicename.split("[")[0]))
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address + " Type",
                                    value=client.client_type))
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address + " Current",
                                    value=client.active_tabpage))
            diagnostic_status_clients.values.append(KeyValue(
                                    key=client.address + " Tabpages",
                                    value=", ".join(client.tabpages)))
        if len(self.clients) == 0:
            diagnostic_status_clients.message = "No clients detected"
        msg.status.append(diagnostic_status_clients)

        # For each registered tabpage handler, get a DiagnosticStatus message.
        for tabpage in self.tabpage_handlers.itervalues():
            msg.status.append(tabpage.cb_diagnostics_update())

        # Publish
        self.diagnostics_pub.publish(msg)
        reactor.callLater(1.0, self.cb_diagnostics_update)
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)
Пример #48
0
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)
Пример #49
0
    def publish(self, sensor_state, gyro):
        curr_time = sensor_state.header.stamp
        # limit to 5hz
        if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
            return
        self.last_diagnostics_time = curr_time

        diag = DiagnosticArray()
        diag.header.stamp = curr_time
        stat = DiagnosticStatus()

        #node status
        stat = DiagnosticStatus(name="WheeledRobin Node", level=DiagnosticStatus.OK, message="RUNNING")
        diag.status.append(stat)

        #mode info
        """
	stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK)
        try:
            stat.message = self.mode[sensor_state.mode]
        except KeyError as ex:
            stat.level=DiagnosticStatus.ERROR
            stat.message = "Invalid Mode Reported %s"%ex
            rospy.logwarn(stat.message)
        diag.status.append(stat)
	"""
        
        #cliff sensors
        """ 
        stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
        if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
            stat.level = DiagnosticStatus.WARN
            if (sensor_state.current/1000.0)>0:
                stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
                stat.level = DiagnosticStatus.OK # We're OK here
            else:
                stat.message = "Near Cliff"
        stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
                       KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
                       KeyValue("Front Left", str(sensor_state.cliff_front_left)),
                       KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
                       KeyValue("Front Right", str(sensor_state.cliff_right)),
                       KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
                       KeyValue("Right", str(sensor_state.cliff_front_right)),
                       KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
        diag.status.append(stat)

        #Wall sensors
        stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
        #wall always seems to be false??? 
        if sensor_state.wall:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Near Wall"
        stat.values = [KeyValue("Wall", str(sensor_state.wall)),
                       KeyValue("Wall Signal", str(sensor_state.wall_signal)),
                       KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
        diag.status.append(stat)

        #Gyro
        stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
        if gyro is None:
            stat.level = DiagnosticStatus.WARN
            stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the wheeled_robin_node."   
        elif gyro.cal_offset < 100:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
        elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
            stat.level = DiagnosticStatus.ERROR
            stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."

        if gyro is not None:    
            stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
                           KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
                           KeyValue("Calibration Offset", str(gyro.cal_offset)),
                           KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
        diag.status.append(stat)
        """
        #Digital Outputs
        stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK")
        out_byte = sensor_state.digital_outputs
        stat.values = [KeyValue("Raw Byte", str(out_byte)),
			KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>0)%2]),
			KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]),
			KeyValue("Digital Out 2", self.digital_outputs[(out_byte >>2)%2]),
			KeyValue("Digital Out 3", self.digital_outputs[(out_byte >>3)%2]),
			KeyValue("Digital Out 4", self.digital_outputs[(out_byte >>4)%2]),
			KeyValue("Digital Out 5", self.digital_outputs[(out_byte >>5)%2]),
			KeyValue("Digital Out 6", self.digital_outputs[(out_byte >>6)%2]),
			KeyValue("Digital Out 7", self.digital_outputs[(out_byte >>7)%2])]
        diag.status.append(stat)

        #Digital Inputs
        stat = DiagnosticStatus(name="Digital Inputs", level = DiagnosticStatus.OK, message = "OK")
        in_byte = sensor_state.digital_inputs
        stat.values = [KeyValue("Raw Byte", str(in_byte)),
			KeyValue("Digital Input 0", self.digital_outputs[(in_byte >>0)%2]),
			KeyValue("Digital Input 1", self.digital_outputs[(in_byte >>1)%2]),
			KeyValue("Digital Input 2", self.digital_outputs[(in_byte >>2)%2]),
			KeyValue("Digital Input 3", self.digital_outputs[(in_byte >>3)%2]),
			KeyValue("Digital Input 4", self.digital_outputs[(in_byte >>4)%2]),
			KeyValue("Digital Input 5", self.digital_outputs[(in_byte >>5)%2]),
			KeyValue("Digital Input 6", self.digital_outputs[(in_byte >>6)%2]),
			KeyValue("Digital Input 7", self.digital_outputs[(in_byte >>7)%2])]
        diag.status.append(stat)

        #publish
        self.diag_pub.publish(diag)