nav_estop.py
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


art_teleop
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:43:08