diagnostics_publisher.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #
00015 #  Copyright © 2012 Clearpath Robotics, Inc. 
00016 #  All Rights Reserved
00017 #  
00018 # Redistribution and use in source and binary forms, with or without
00019 # modification, are permitted provided that the following conditions are met:
00020 #     * Redistributions of source code must retain the above copyright
00021 #       notice, this list of conditions and the following disclaimer.
00022 #     * Redistributions in binary form must reproduce the above copyright
00023 #       notice, this list of conditions and the following disclaimer in the
00024 #       documentation and/or other materials provided with the distribution.
00025 #     * Neither the name of Clearpath Robotics, Inc. nor the
00026 #       names of its contributors may be used to endorse or promote products
00027 #       derived from this software without specific prior written permission.
00028 # 
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00030 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00031 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00032 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00033 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00034 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00036 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00037 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00038 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00039 #
00040 # Please send comments, questions, or patches to skynet@clearpathrobotics.com
00041 #
00042 
00043 # ROS
00044 import rospy
00045 import applanix_msgs.msg
00046 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00047 
00048 
00049 class BitfieldRepublisher(object):
00050   def __init__(self, topic_name, topic_type, fields):
00051     flags = []
00052     attrs = dir(applanix_msgs.msg.GeneralStatus)
00053     for field in fields:
00054       prefix = field.upper() + "_"
00055       field_flags = []
00056       for attr in attrs:
00057         if attr.startswith(prefix):
00058           short_name = attr[len(prefix):len(attr)]
00059           mask = getattr(applanix_msgs.msg.GeneralStatus, attr)
00060           field_flags.append((mask, short_name))
00061       field_flags.sort()
00062       flags.append((field, tuple(field_flags)))
00063     self.flags = tuple(flags)
00064 
00065     self.status_msg = DiagnosticArray()
00066     self.status_msg.status.append(DiagnosticStatus(
00067       level = DiagnosticStatus.OK,
00068       name = "Applanix AP10",
00069       message = "OK"))
00070     rospy.Subscriber(topic_name, topic_type, self._cb)
00071     self.pub = rospy.Publisher("/diagnostics", DiagnosticArray, latch=True)
00072 
00073   def _cb(self, msg):
00074     self.status_msg.status[0].values = []
00075     for field, field_flags in self.flags:
00076       field_value = getattr(msg, field)
00077       for mask, flag in field_flags:
00078         value = str(int((field_value & mask) != 0))
00079         self.status_msg.status[0].values.append(KeyValue(flag, value))
00080     self.pub.publish(self.status_msg) 
00081 
00082 
00083 def main():
00084   rospy.init_node('applanix_diagnostics')
00085   BitfieldRepublisher("status/general", applanix_msgs.msg.GeneralStatus, \
00086                       ('status_a', 'status_b', 'status_c', 'fdir_1', \
00087                        'fdir_2', 'fdir_3', 'fdir_4', 'fdir_5', 'extended'))
00088   rospy.spin()


applanix_diagnostics
Author(s): Administrator
autogenerated on Thu Jan 2 2014 11:05:26