monitor.py

Go to the documentation of this file.

#! /usr/bin/python
#***********************************************************
#* Software License Agreement (BSD License)
#*
#*  Copyright (c) 2009, Willow Garage, Inc.
#*  All rights reserved.
#*
#*  Redistribution and use in source and binary forms, with or without
#*  modification, are permitted provided that the following conditions
#*  are met:
#*
#*   * Redistributions of source code must retain the above copyright
#*     notice, this list of conditions and the following disclaimer.
#*   * Redistributions in binary form must reproduce the above
#*     copyright notice, this list of conditions and the following
#*     disclaimer in the documentation and/or other materials provided
#*     with the distribution.
#*   * Neither the name of Willow Garage, Inc. nor the names of its
#*     contributors may be used to endorse or promote products derived
#*     from this software without specific prior written permission.
#*
#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#*  POSSIBILITY OF SUCH DAMAGE.
#* 
#* Author: Eitan Marder-Eppstein
#***********************************************************
from __future__ import with_statement

PKG='continuous_ops_alerts'
import roslib; roslib.load_manifest(PKG)

import rospy
from googlevoice import Voice
from googlevoice.util import input

import tf2
import tf2_ros

import math

from geometry_msgs.msg import PoseStamped
from pr2_msgs.msg import PowerState
from std_msgs.msg import Bool
import tf2_geometry_msgs

import threading

class ContinuousOpsMonitor:
    def __init__(self, voice):
        self.voice = voice
        self.tf_client = tf2_ros.BufferClient('tf2_buffer_server')
        rospy.loginfo('Continuous ops monitor waiting for tf server')
        self.tf_client.wait_for_server()
        rospy.loginfo('Continuous ops monitor connected to tf server')

        self.move_distance = rospy.get_param('~move_distance', 2.0)
        self.move_timeout = rospy.get_param('~move_timeout', 300.0)
        #self.phone_numbers = ['8472745914', '6505294522', '6263729726']
        self.phone_numbers = ['5107100513', '6505294522']
        
        self.last_pose = PoseStamped()
        self.last_pose.header.stamp = rospy.Time.now()

        self.mutex = threading.Lock()
        self.plugged_in = True

        self.power_sub = rospy.Subscriber('power_state', PowerState, self.power_cb)
        self.motor_sub = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self.motor_cb)
        self.motors_halted = False


    def motor_cb(self, msg):
        with self.mutex:
            if msg.data:
                if not self.motors_halted:
                    self.send_sms('My leg hurts, by casters suck, my motors halted... ')
                self.motors_halted = True
            else:
                self.motors_halted = False

    def power_cb(self, msg):
        with self.mutex:
            if msg.AC_present == 0:
                if self.plugged_in:
                    self.last_pose.header.stamp = rospy.Time.now()
                self.plugged_in = False
                
            else:
                self.plugged_in = True

    def distance(self, p1, p2):
        dx = p1.pose.position.x - p2.pose.position.x
        dy = p1.pose.position.y - p2.pose.position.y
        distance = math.sqrt(dx * dx + dy * dy)
        return distance

    def distance_check(self):
        base_pose = PoseStamped()
        base_pose.header.stamp = rospy.Time(0.0)
        base_pose.header.frame_id = 'base_link'
        base_pose.pose.orientation.w = 1.0

        try:
            map_pose = self.tf_client.transform(base_pose, 'map')
            dist = self.distance(map_pose, self.last_pose)

            #If we've moved far enough, then we'll update our last pose and return
            if dist > self.move_distance:
                self.last_pose = map_pose
                return

            #If we haven't moved far enough, we need to check if we've timed out
            if self.last_pose.header.stamp + rospy.Duration(self.move_timeout) < rospy.Time.now():
                self.send_sms('Robot has not moved in %s seconds, please check if it needs help.' % self.move_timeout)
                #Don't send too many sms messages, we'll reset our timer
                self.last_pose.header.stamp = rospy.Time.now()

        except tf2.TransformException as e:
            rospy.logerr("%s" % e)

    def send_sms(self, text):
        for number in self.phone_numbers:
            self.voice.send_sms(number, text)

    def run(self):
        r = rospy.Rate(0.1)
        while not rospy.is_shutdown():
            with self.mutex:
                if not self.plugged_in:
                    #check that things are OK here
                    self.distance_check()
            r.sleep()

if __name__ == '__main__':
    rospy.init_node('continuous_ops_alert')
    voice = Voice()
    voice.login()
    com = ContinuousOpsMonitor(voice)

    try:
        com.run()
    except rospy.ROSInterruptException: pass