diagnostics.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD)
00005 #
00006 #  file      @publisher.py
00007 #  authors   Mike Purvis <mpurvis@clearpathrobotics.com>
00008 #            NovAtel <novatel.com/support>
00009 #  copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved.
00010 #            Copyright (c) 2014, NovAtel Inc., All rights reserved.
00011 #
00012 # Redistribution and use in source and binary forms, with or without modification, are permitted provided that
00013 # the following conditions are met:
00014 #  * Redistributions of source code must retain the above copyright notice, this list of conditions and the
00015 #    following disclaimer.
00016 #  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
00017 #    following disclaimer in the documentation and/or other materials provided with the distribution.
00018 #  * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
00019 #    products derived from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
00022 # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
00023 # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
00024 # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00025 # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 import rospy
00030 import diagnostic_updater
00031 
00032 from diagnostic_msgs.msg import DiagnosticStatus
00033 from novatel_msgs.msg import BESTPOS, INSPVAX
00034 
00035 
00036 class NovatelDiagnostics(object):
00037     def __init__(self):
00038         self.last_bestpos = None
00039         self.last_inspvax = None
00040         rospy.Subscriber("novatel_data/bestpos", BESTPOS, self.bestpos_callback)
00041         rospy.Subscriber("novatel_data/inspvax", INSPVAX, self.inspvax_callback)
00042 
00043         self.updater = diagnostic_updater.Updater()
00044         self.updater.setHardwareID("none")
00045         self.updater.add("Novatel SPAN", self.produce_diagnostics)
00046 
00047     def bestpos_callback(self, msg):
00048         self.last_bestpos = msg
00049         self.updater.setHardwareID("firmware-%d" % msg.header.software_version)
00050         self.updater.update()
00051 
00052     def inspvax_callback(self, msg):
00053         self.last_inspvax = msg
00054         self.updater.update()
00055 
00056     @staticmethod
00057     def get_status_string(msg, field):
00058         value = getattr(msg, field)
00059         matching_status = [x[len(field) + 1:] for x in dir(msg) if x.startswith(field.upper()) and
00060                            value == getattr(msg, x)]
00061         if len(matching_status) != 1:
00062             return "No matching constant"
00063         return matching_status[0]
00064 
00065     @staticmethod
00066     def get_status_bitfield(msg, field):
00067         value = getattr(msg, field)
00068         matching_statuses = [x[len(field) + 1:] for x in dir(msg) if x.startswith(field.upper()) and
00069                              value & getattr(msg, x)]
00070         return ', '.join(matching_statuses)
00071 
00072     def produce_diagnostics(self, stat):
00073         if self.last_bestpos:
00074             stat.add("GNSS Solution Status",
00075                      self.get_status_string(self.last_bestpos, "solution_status"))
00076             stat.add("GNSS Position Type",
00077                      self.get_status_string(self.last_bestpos, "position_type"))
00078             self.last_bestpos = None
00079 
00080         if self.last_inspvax:
00081             if self.last_inspvax.ins_status != INSPVAX.INS_STATUS_SOLUTION_GOOD:
00082                 stat.summary(DiagnosticStatus.WARN, "INS Solution not GOOD.")
00083             elif self.last_inspvax.position_type != INSPVAX.POSITION_TYPE_PPP:
00084                 stat.summary(DiagnosticStatus.WARN, "INS Position type not PPP.")
00085             else:
00086                 stat.summary(DiagnosticStatus.OK, "INS Solution GOOD, PPP fix present.")
00087 
00088             stat.add("INS Solution Status",
00089                      self.get_status_string(self.last_inspvax, "ins_status"))
00090             stat.add("INS Position Type",
00091                      self.get_status_string(self.last_inspvax, "position_type"))
00092             stat.add("INS Extended Status",
00093                      self.get_status_bitfield(self.last_inspvax, "extended_status"))
00094             stat.add("Seconds since last ZUPT or position update.",
00095                      self.last_inspvax.seconds_since_update)
00096             stat.add("Receiver Status",
00097                      self.get_status_bitfield(self.last_inspvax.header, "receiver_status"))
00098             self.last_inspvax = None
00099         else:
00100             stat.summary(DiagnosticStatus.ERROR,
00101                          "No INSPVAX logs received from Novatel system.")
00102 
00103         return stat


novatel_span_driver
Author(s): NovAtel Support
autogenerated on Fri Aug 28 2015 11:47:11