$search
00001 #!/usr/bin/env python 00002 # 00003 # ART vehicle navigator E-stop interface for teleoperation 00004 # 00005 # Copyright (C) 2011 Austin Robot Technology 00006 # License: Modified BSD Software License Agreement 00007 # 00008 # $Id: nav_estop.py 1161 2011-03-26 02:10:49Z jack.oquin $ 00009 00010 PKG_NAME = 'art_teleop' 00011 import roslib; 00012 roslib.load_manifest(PKG_NAME) 00013 import rospy 00014 00015 from art_msgs.msg import Behavior 00016 from art_msgs.msg import EstopState 00017 from art_msgs.msg import NavigatorCommand 00018 from art_msgs.msg import NavigatorState 00019 00020 class EstopNavigator(): 00021 "ART navigator E-stop control interface." 00022 00023 def __init__(self, use_navigator=True): 00024 "constructor: binds to ROS topics" 00025 self.use_navigator = use_navigator 00026 self.last_state_ = EstopState() 00027 self.new_state_ = EstopState() 00028 self.new_behavior_ = Behavior.NONE 00029 self.cmd = rospy.Publisher('navigator/cmd', NavigatorCommand) 00030 self.topic = rospy.Subscriber('navigator/state', NavigatorState, 00031 self.check_state) 00032 00033 def check_state(self, state_msg): 00034 "check navigator state, request change if not desired state" 00035 self.last_state_ = state_msg.estop.state 00036 rospy.logdebug('E-stop state: ' + str(self.last_state_)) 00037 if (self.last_state_ != self.new_state_ 00038 and self.new_behavior_ != Behavior.NONE): 00039 # send navigator command msg requesting new behavior 00040 cmd_msg = NavigatorCommand() 00041 cmd_msg.header.stamp = rospy.Time.now() 00042 cmd_msg.header.frame_id = '/map' 00043 cmd_msg.order.behavior.value = self.new_behavior_ 00044 self.cmd.publish(cmd_msg) 00045 00046 def pause(self): 00047 "request immediate stop" 00048 self.new_state_ = EstopState.Pause 00049 self.new_behavior_ = Behavior.Pause 00050 rospy.loginfo('Stopping') 00051 00052 def run(self): 00053 "request autonomous running" 00054 self.new_state_ = EstopState.Run 00055 self.new_behavior_ = Behavior.Run 00056 rospy.loginfo('Running autonomously') 00057 00058 def suspend(self): 00059 "request suspension of autonomous operation" 00060 self.new_state_ = EstopState.Suspend 00061 self.new_behavior_ = Behavior.Suspend 00062 rospy.loginfo('Suspending autonomous operation') 00063 00064 def is_suspended(self): 00065 "return True if navigator operation suspended (or not using navigator)" 00066 return (not self.use_navigator 00067 or (self.last_state_ == EstopState.Suspend)) 00068 00069 # standalone test of package 00070 if __name__ == '__main__': 00071 00072 rospy.init_node('nav_estop') 00073 00074 # make an instance 00075 est = EstopNavigator() 00076 00077 # run the program 00078 try: 00079 rospy.spin() 00080 except rospy.ROSInterruptException: pass