recharge_application.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #    * Redistributions of source code must retain the above copyright
00012 #        notice, this list of conditions and the following disclaimer.
00013 #    * Redistributions in binary form must reproduce the above
00014 #        copyright notice, this list of conditions and the following
00015 #        disclaimer in the documentation and/or other materials provided
00016 #        with the distribution.
00017 #    * Neither the name of the Willow Garage nor the names of its
00018 #        contributors may be used to endorse or promote products derived
00019 #        from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import roslib
00035 roslib.load_manifest('pr2_plugs_actions')
00036 
00037 import rospy
00038 import actionlib
00039 from random import choice
00040 import battery
00041 
00042 from pr2_plugs_msgs.msg import EmptyAction, RechargeAction, RechargeGoal, RechargeCommand, RechargeState
00043 
00044 class Application():
00045     def __init__(self):
00046         self.charging_locations = rospy.get_param("~charging_locations", ['wim', 'whiteboard', 'james'])
00047 
00048         self.recharge = actionlib.SimpleActionClient('recharge', RechargeAction)        
00049         self.recharge.wait_for_server()
00050 
00051         self.app_action = actionlib.SimpleActionServer('recharge_application_server', EmptyAction, self.application_cb) 
00052 
00053     def plug_in(self):
00054         if self.state == 'unplugged':
00055             goal = RechargeGoal()
00056             goal.command.command = RechargeCommand.PLUG_IN
00057             goal.command.plug_id = choice(self.charging_locations)
00058             res = self.recharge.send_goal_and_wait(goal) == actionlib.GoalStatus.SUCCEEDED
00059             if res:
00060                 self.state = 'plugged_in'
00061             return res
00062         else:
00063             rospy.logerr('Calling plug_in when robot is not unplugged')
00064             return False
00065 
00066 
00067     def unplug(self):
00068         if self.state == 'plugged_in':
00069             goal = RechargeGoal()
00070             goal.command.command = RechargeCommand.UNPLUG
00071             self.recharge.send_goal_and_wait(goal)
00072             self.state = 'unplugged'
00073         else:
00074             rospy.logerr('Calling unplug when robot is not plugged in')
00075 
00076 
00077 
00078     def application_cb(self, goal):
00079         rospy.loginfo('Recharge application became active.')    
00080         plugged_in = False
00081         self.state = 'unplugged'
00082 
00083         # plug in robot
00084         while rospy.is_shutdown and not plugged_in and not self.app_action.is_preempt_requested():
00085             if self.plug_in():
00086                 if not self.app_action.is_preempt_requested():
00087                     rospy.loginfo('I think I successfully plugged myself in. But need to verify this...')
00088                     battery_monitor = battery.Battery()
00089                     if battery_monitor.get_state() == 'charging':
00090                         rospy.loginfo("Yes, we're plugged in, I can feel my batteries getting charged.") 
00091                         plugged_in = True
00092                     else:
00093                         rospy.logerr('Plugging in failed even though the action claimed it succeeded. Will try again, but first need to unplug.') 
00094                         self.unplug()
00095             else:
00096                 rospy.logerr("Plugging reported failure. Trying again")
00097 
00098 
00099         # check for preemption while plugged in
00100         while not rospy.is_shutdown():
00101             rospy.sleep(0.1)
00102             if self.app_action.is_preempt_requested():
00103                 break
00104 
00105         # unplug robot if it is plugged in
00106         if self.state == 'plugged_in':
00107             self.unplug()
00108         self.app_action.set_preempted()
00109 
00110 
00111 
00112 
00113 def main():
00114     rospy.init_node("recharge_application_monitor")
00115 
00116     a = Application()
00117     rospy.spin()
00118     
00119 
00120 if __name__ == "__main__":
00121     main()
00122 


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