pr2_base_recovery.py

Go to the documentation of this file.

#!/usr/bin/python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, 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 the Willow Garage 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: Wim Meeussen

import roslib; roslib.load_manifest('pr2_base_recovery')
import rospy
import random
import PyKDL
from math import fabs
from threading import Lock
from geometry_msgs.msg import Twist
from pr2_mechanism_controllers.msg import Odometer, BaseControllerState2
from threading import Thread

DIST_THRESHOLD = 0.0001
EFF_THRESHOLD = 3.0
BASE_THRESHOLD = 0.05
TIMEOUT = 10.0
RECEIVE_TIMEOUT = 2.0
RECOVERY_DURATION = 0.25

class BaseRecovery(Thread):
    def __init__(self):
        Thread.__init__(self)
        self.lock = Lock()
        self.odom_sub = rospy.Subscriber('base_odometry/odometer', Odometer, self.odom_cb)
        self.control_sub = rospy.Subscriber('base_controller/state', BaseControllerState2, self.control_cb)
        self.base_sub = rospy.Subscriber('base_controller/command', Twist, self.base_cb)
        self.eff_start_time = rospy.Time.now()
        self.dist_start_time = rospy.Time.now()
        self.base_start_time = rospy.Time.now()
        self.base_receive_time = rospy.Time(0.0)
        self.dist = 0

    def run(self):
        recovering = last_recovering = False
        recovery_start_time = rospy.Time(0.0)
        timeout = rospy.Duration(TIMEOUT)
        receive_timeout = rospy.Duration(RECEIVE_TIMEOUT)
        recovery_duration = rospy.Duration(RECOVERY_DURATION)
        recovery_twist = Twist()
        vel_pub = rospy.Publisher('base_controller/command', Twist)

        while not rospy.is_shutdown():
            now = rospy.Time.now()
            with self.lock:

                # check if need to start a recovery
                if now < self.base_receive_time + receive_timeout and  now > self.base_start_time + timeout and now > self.eff_start_time + timeout and now > self.dist_start_time + timeout:
                    vec = PyKDL.Vector(random.randint(-50, 50), random.randint(-50, 50), random.randint(-100, 100))
                    vec.Normalize()
                    recovery_twist.linear.x = vec[0]
                    recovery_twist.linear.y = vec[1]
                    recovery_twist.angular.z = vec[2]
                    recovery_start_time = now
                    self.eff_start_time = now
                    self.dist_start_time = now
                    rospy.logerr("Base is stuck... recovering with velocity (%f %f %f)"%(vec[0], vec[1], vec[2]))

                # do the recovery
                if now < recovery_start_time + recovery_duration:
                    vel_pub.publish(recovery_twist)
                    recovering = True
                else:
                    recovering = False
                
                # after recovery send zero twist
                if not recovering and last_recovering:
                    vel_pub.publish(Twist())                    
                last_recovering = recovering

            if recovering:
                rospy.sleep(0.001)
            else:
                rospy.sleep(0.1)


    def odom_cb(self, msg):
        with self.lock:
            dist = msg.distance + (msg.angle * 0.25)
            if dist > self.dist + DIST_THRESHOLD:
                #print 'resetting odom monitor'
                self.dist_start_time = rospy.Time.now()
                self.dist = dist

    def base_cb(self, msg):
        with self.lock:
            self.base_receive_time = rospy.Time.now()
            base_cmd = fabs(msg.linear.x) + fabs(msg.linear.y) + fabs(msg.angular.z)
            if base_cmd < BASE_THRESHOLD:
                #print 'resetting base monitor'
                self.base_start_time = rospy.Time.now()

    def control_cb(self, msg):
        with self.lock:
            eff = max( (fabs(e) for e in msg.joint_effort_measured) )
            if eff < EFF_THRESHOLD:
                #print 'resetting effort monitor'
                self.eff_start_time = rospy.Time.now()



def main():
    rospy.init_node('continuous_ops_executive')
    random.seed()
    br = BaseRecovery()
    br.start()
    rospy.spin()


if __name__ == '__main__':
    main()