monitor.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: Eitan Marder-Eppstein
00036 #***********************************************************
00037 from __future__ import with_statement
00038 
00039 PKG='continuous_ops_alerts'
00040 import roslib; roslib.load_manifest(PKG)
00041 
00042 import rospy
00043 from googlevoice import Voice
00044 from googlevoice.util import input
00045 
00046 import tf2
00047 import tf2_ros
00048 
00049 import math
00050 
00051 from geometry_msgs.msg import PoseStamped
00052 from pr2_msgs.msg import PowerState
00053 from std_msgs.msg import Bool
00054 import tf2_geometry_msgs
00055 
00056 import threading
00057 
00058 class ContinuousOpsMonitor:
00059     def __init__(self, voice):
00060         self.voice = voice
00061         self.tf_client = tf2_ros.BufferClient('tf2_buffer_server')
00062         rospy.loginfo('Continuous ops monitor waiting for tf server')
00063         self.tf_client.wait_for_server()
00064         rospy.loginfo('Continuous ops monitor connected to tf server')
00065 
00066         self.move_distance = rospy.get_param('~move_distance', 2.0)
00067         self.move_timeout = rospy.get_param('~move_timeout', 300.0)
00068         #self.phone_numbers = ['8472745914', '6505294522', '6263729726']
00069         self.phone_numbers = ['5107100513', '6505294522']
00070         
00071         self.last_pose = PoseStamped()
00072         self.last_pose.header.stamp = rospy.Time.now()
00073 
00074         self.mutex = threading.Lock()
00075         self.plugged_in = True
00076 
00077         self.power_sub = rospy.Subscriber('power_state', PowerState, self.power_cb)
00078         self.motor_sub = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self.motor_cb)
00079         self.motors_halted = False
00080 
00081 
00082     def motor_cb(self, msg):
00083         with self.mutex:
00084             if msg.data:
00085                 if not self.motors_halted:
00086                     self.send_sms('My leg hurts, by casters suck, my motors halted... ')
00087                 self.motors_halted = True
00088             else:
00089                 self.motors_halted = False
00090 
00091     def power_cb(self, msg):
00092         with self.mutex:
00093             if msg.AC_present == 0:
00094                 if self.plugged_in:
00095                     self.last_pose.header.stamp = rospy.Time.now()
00096                 self.plugged_in = False
00097                 
00098             else:
00099                 self.plugged_in = True
00100 
00101     def distance(self, p1, p2):
00102         dx = p1.pose.position.x - p2.pose.position.x
00103         dy = p1.pose.position.y - p2.pose.position.y
00104         distance = math.sqrt(dx * dx + dy * dy)
00105         return distance
00106 
00107     def distance_check(self):
00108         base_pose = PoseStamped()
00109         base_pose.header.stamp = rospy.Time(0.0)
00110         base_pose.header.frame_id = 'base_link'
00111         base_pose.pose.orientation.w = 1.0
00112 
00113         try:
00114             map_pose = self.tf_client.transform(base_pose, 'map')
00115             dist = self.distance(map_pose, self.last_pose)
00116 
00117             #If we've moved far enough, then we'll update our last pose and return
00118             if dist > self.move_distance:
00119                 self.last_pose = map_pose
00120                 return
00121 
00122             #If we haven't moved far enough, we need to check if we've timed out
00123             if self.last_pose.header.stamp + rospy.Duration(self.move_timeout) < rospy.Time.now():
00124                 self.send_sms('Robot has not moved in %s seconds, please check if it needs help.' % self.move_timeout)
00125                 #Don't send too many sms messages, we'll reset our timer
00126                 self.last_pose.header.stamp = rospy.Time.now()
00127 
00128         except tf2.TransformException as e:
00129             rospy.logerr("%s" % e)
00130 
00131     def send_sms(self, text):
00132         for number in self.phone_numbers:
00133             self.voice.send_sms(number, text)
00134 
00135     def run(self):
00136         r = rospy.Rate(0.1)
00137         while not rospy.is_shutdown():
00138             with self.mutex:
00139                 if not self.plugged_in:
00140                     #check that things are OK here
00141                     self.distance_check()
00142             r.sleep()
00143 
00144 if __name__ == '__main__':
00145     rospy.init_node('continuous_ops_alert')
00146     voice = Voice()
00147     voice.login()
00148     com = ContinuousOpsMonitor(voice)
00149 
00150     try:
00151         com.run()
00152     except rospy.ROSInterruptException: pass
00153 


continuous_ops_alerts
Author(s): Eitan Marder-Eppstein
autogenerated on Fri Dec 6 2013 19:59:13