diagnostics.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # Software License Agreement (BSD)
5 #
6 # file @publisher.py
7 # authors Mike Purvis <mpurvis@clearpathrobotics.com>
8 # NovAtel <novatel.com/support>
9 # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved.
10 # Copyright (c) 2014, NovAtel Inc., All rights reserved.
11 #
12 # Redistribution and use in source and binary forms, with or without modification, are permitted provided that
13 # the following conditions are met:
14 # * Redistributions of source code must retain the above copyright notice, this list of conditions and the
15 # following disclaimer.
16 # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
17 # following disclaimer in the documentation and/or other materials provided with the distribution.
18 # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
19 # products derived from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
22 # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
24 # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
25 # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 
29 import rospy
30 import diagnostic_updater
31 
32 from diagnostic_msgs.msg import DiagnosticStatus
33 from novatel_msgs.msg import BESTPOS, INSPVAX
34 
35 
36 class NovatelDiagnostics(object):
37  def __init__(self):
38  self.last_bestpos = None
39  self.last_inspvax = None
40  rospy.Subscriber("novatel_data/bestpos", BESTPOS, self.bestpos_callback)
41  rospy.Subscriber("novatel_data/inspvax", INSPVAX, self.inspvax_callback)
42 
44  self.updater.setHardwareID("none")
45  self.updater.add("Novatel SPAN", self.produce_diagnostics)
46 
47  def bestpos_callback(self, msg):
48  self.last_bestpos = msg
49  self.updater.setHardwareID("firmware-%d" % msg.header.software_version)
50  self.updater.update()
51 
52  def inspvax_callback(self, msg):
53  self.last_inspvax = msg
54  self.updater.update()
55 
56  @staticmethod
57  def get_status_string(msg, field):
58  value = getattr(msg, field)
59  matching_status = [x[len(field) + 1:] for x in dir(msg) if x.startswith(field.upper()) and
60  value == getattr(msg, x)]
61  if len(matching_status) != 1:
62  return "No matching constant"
63  return matching_status[0]
64 
65  @staticmethod
66  def get_status_bitfield(msg, field):
67  value = getattr(msg, field)
68  matching_statuses = [x[len(field) + 1:] for x in dir(msg) if x.startswith(field.upper()) and
69  value & getattr(msg, x)]
70  return ', '.join(matching_statuses)
71 
72  def produce_diagnostics(self, stat):
73  if self.last_bestpos:
74  stat.add("GNSS Solution Status",
75  self.get_status_string(self.last_bestpos, "solution_status"))
76  stat.add("GNSS Position Type",
77  self.get_status_string(self.last_bestpos, "position_type"))
78  self.last_bestpos = None
79 
80  if self.last_inspvax:
81  if self.last_inspvax.ins_status != INSPVAX.INS_STATUS_SOLUTION_GOOD:
82  stat.summary(DiagnosticStatus.WARN, "INS Solution not GOOD.")
83  elif self.last_inspvax.position_type != INSPVAX.POSITION_TYPE_PPP:
84  stat.summary(DiagnosticStatus.WARN, "INS Position type not PPP.")
85  else:
86  stat.summary(DiagnosticStatus.OK, "INS Solution GOOD, PPP fix present.")
87 
88  stat.add("INS Solution Status",
89  self.get_status_string(self.last_inspvax, "ins_status"))
90  stat.add("INS Position Type",
91  self.get_status_string(self.last_inspvax, "position_type"))
92  stat.add("INS Extended Status",
93  self.get_status_bitfield(self.last_inspvax, "extended_status"))
94  stat.add("Seconds since last ZUPT or position update.",
95  self.last_inspvax.seconds_since_update)
96  stat.add("Receiver Status",
97  self.get_status_bitfield(self.last_inspvax.header, "receiver_status"))
98  self.last_inspvax = None
99  else:
100  stat.summary(DiagnosticStatus.ERROR,
101  "No INSPVAX logs received from Novatel system.")
102 
103  return stat


novatel_span_driver
Author(s): NovAtel Support
autogenerated on Mon Sep 2 2019 03:58:19