battery.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, 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 #* Author: Wim Meeussen
00036 #***********************************************************
00037 from __future__ import with_statement
00038 
00039 import roslib; roslib.load_manifest('pr2_plugs_actions')
00040 import rospy
00041 import threading
00042 import copy
00043 from std_msgs.msg import Bool
00044 from pr2_msgs.msg import PowerState
00045 from collections import deque
00046 
00047 
00048 class Battery:
00049     def __init__(self, low_cb=None, high_cb=None, name='battery_monitor'):
00050         self.lock = threading.Lock()  
00051         self.state = None
00052         self.low_trigger = 0
00053         self.high_trigger = 100
00054         self.low_cb = low_cb
00055         self.high_cb = high_cb
00056         self.buffer_size = rospy.get_param("~%s/buffer_size"%name, 5)
00057         self.msgs = deque()
00058         self.power_sub = rospy.Subscriber('power_state', PowerState, self._power_cb)
00059         rospy.loginfo("Start monitoring battery")
00060 
00061     def set_thresholds(self, low=None, high=None):
00062         with self.lock:
00063             if low:
00064                 self.low_trigger = low
00065             if high:
00066                 self.high_trigger = high
00067             self.state = None
00068             rospy.loginfo("Set battery thresholds to %f and %f"%(self.low_trigger, self.high_trigger))
00069 
00070 
00071     def get_state(self):
00072         while not rospy.is_shutdown():
00073             rospy.sleep(0.1)
00074             with self.lock:
00075                 if self.state:
00076                     return copy.deepcopy(self.state)
00077 
00078 
00079 
00080     def _power_cb(self, msg):
00081         with self.lock:
00082             self.msgs.append(msg)
00083 
00084             # make sure that the queue doesn't go over length
00085             while len(self.msgs) > self.buffer_size:
00086                 self.msgs.popleft()
00087 
00088             # dont't do anything if buffer is not filled up
00089             if not len(self.msgs) >= self.buffer_size:
00090                 return 
00091 
00092             # check for low/high transitions
00093             state = 0
00094             for msg in self.msgs:
00095                 if msg.relative_capacity <= self.low_trigger:
00096                     state -= 1
00097                 elif msg.relative_capacity >= self.high_trigger:
00098                     state += 1
00099 
00100             # send high trigger only if ac is plugged in
00101             if self.high_cb and state == self.buffer_size and self.msgs[-1].AC_present == 1:
00102                 self.msgs = deque()  # clear queue to avoid triggering again in the next callback
00103                 if self.state != 'high':
00104                     rospy.loginfo('Calling high battery callback')
00105                     self.high_cb()
00106                 self.state = 'high'
00107 
00108             # send low trigger only if ac is not plugged in
00109             elif self.low_cb and state == -self.buffer_size and self.msgs[-1].AC_present != 1:
00110                 self.msgs = deque()  # clear queue to avoid triggering again in the next callback
00111                 if self.state != 'low':
00112                     rospy.loginfo('Calling low battery callback')
00113                     self.low_cb()
00114                 self.state = 'low'
00115 
00116             elif self.msgs[-1].AC_present == 1:
00117                 self.state = 'charging'
00118 
00119             elif self.msgs[-1].AC_present != 1:
00120                 self.state = 'discharging'
00121 
00122 
00123 


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13