dashboard_aggregator.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Willow Garage, Inc. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 import time
00036 import rospy
00037 from std_msgs.msg import Bool
00038 from pr2_msgs.msg import PowerBoardState, PowerState, DashboardState, AccessPoint
00039 
00040 class DashboardAggregator:
00041   def __init__(self):
00042     self.msg = DashboardState()
00043 
00044     # Create publisher
00045     self.pub = rospy.Publisher("dashboard_agg", DashboardState, queue_size=10)
00046 
00047     # Create subscribers
00048     # Circuit Breaker
00049     rospy.Subscriber("power_board/state", PowerBoardState, self.powerBoardCB)
00050     self.last_power_board_state = 0
00051     # Battery
00052     rospy.Subscriber("power_state", PowerState, self.powerCB)
00053     self.last_power_state = 0
00054     # Wireless
00055     rospy.Subscriber("ddwrt/accesspoint", AccessPoint, self.accessPointCB)
00056     self.last_access_point = 0
00057     # Motor State
00058     rospy.Subscriber("pr2_etherCAT/motors_halted", Bool, self.motorsHaltedCB)
00059     self.last_motors_halted = 0
00060 
00061   def motorsHaltedCB(self, msg):
00062     self.last_motors_halted = time.time()
00063     self.msg.motors_halted = msg
00064 
00065   def powerBoardCB(self, msg):
00066     self.last_power_board_state = time.time()
00067     self.msg.power_board_state = msg
00068 
00069   def powerCB(self, msg):
00070     self.last_power_state = time.time()
00071     self.msg.power_state = msg
00072 
00073   def accessPointCB(self, msg):
00074     self.last_access_point = time.time()
00075     self.msg.access_point = msg
00076 
00077   def publish(self):
00078     now = time.time()
00079     self.msg.motors_halted_valid = (now - self.last_motors_halted) < 3
00080     self.msg.power_board_state_valid = (now - self.last_power_board_state) < 3
00081     self.msg.power_state_valid = (now - self.last_power_state) < 25
00082     self.msg.access_point_valid = (now - self.last_access_point) < 5
00083     self.pub.publish(self.msg)
00084 
00085 def main():
00086   rospy.init_node("pr2_dashboard_aggregator")
00087   da = DashboardAggregator()
00088   r = rospy.Rate(1)
00089   while not rospy.is_shutdown():
00090     da.publish()
00091     try:
00092       r.sleep()
00093     except rospy.exceptions.ROSInterruptException:
00094       rospy.logdebug('Sleep interrupted')
00095 
00096 if __name__ == "__main__":
00097   main()


pr2_dashboard_aggregator
Author(s): Rob Wheeler, Austin Hendrix
autogenerated on Thu Jun 6 2019 19:03:35