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 roslib; roslib.load_manifest('pr2_dashboard_aggregator')
00036 
00037 import time
00038 import rospy
00039 from std_msgs.msg import Bool
00040 from pr2_msgs.msg import PowerBoardState, PowerState, DashboardState, AccessPoint
00041 
00042 class DashboardAggregator:
00043   def __init__(self):
00044     self.msg = DashboardState()
00045 
00046     # Create publisher
00047     self.pub = rospy.Publisher("dashboard_agg", DashboardState)
00048 
00049     # Create subscribers
00050     # Circuit Breaker
00051     rospy.Subscriber("power_board/state", PowerBoardState, self.powerBoardCB)
00052     self.last_power_board_state = 0
00053     # Battery
00054     rospy.Subscriber("power_state", PowerState, self.powerCB)
00055     self.last_power_state = 0
00056     # Wireless
00057     rospy.Subscriber("ddwrt/accesspoint", AccessPoint, self.accessPointCB)
00058     self.last_access_point = 0
00059     # Motor State
00060     rospy.Subscriber("pr2_etherCAT/motors_halted", Bool, self.motorsHaltedCB)
00061     self.last_motors_halted = 0
00062 
00063   def motorsHaltedCB(self, msg):
00064     self.last_motors_halted = time.time()
00065     self.msg.motors_halted = msg
00066 
00067   def powerBoardCB(self, msg):
00068     self.last_power_board_state = time.time()
00069     self.msg.power_board_state = msg
00070 
00071   def powerCB(self, msg):
00072     self.last_power_state = time.time()
00073     self.msg.power_state = msg
00074 
00075   def accessPointCB(self, msg):
00076     self.last_access_point = time.time()
00077     self.msg.access_point = msg
00078 
00079   def publish(self):
00080     now = time.time()
00081     self.msg.motors_halted_valid = (now - self.last_motors_halted) < 3
00082     self.msg.power_board_state_valid = (now - self.last_power_board_state) < 3
00083     self.msg.power_state_valid = (now - self.last_power_state) < 25
00084     self.msg.access_point_valid = (now - self.last_access_point) < 5
00085     self.pub.publish(self.msg)
00086 
00087 def main():
00088   rospy.init_node("pr2_dashboard_aggregator")
00089   da = DashboardAggregator()
00090   r = rospy.Rate(1)
00091   while not rospy.is_shutdown():
00092     da.publish()
00093     try:
00094       r.sleep()
00095     except rospy.exceptions.ROSInterruptException:
00096       rospy.logdebug('Sleep interrupted')
00097 
00098 if __name__ == "__main__":
00099   main()


pr2_dashboard_aggregator
Author(s): Rob Wheeler
autogenerated on Mon Jan 6 2014 11:34:14