$search
00001 # ROS imports 00002 import roslib 00003 roslib.load_manifest('srs_states') 00004 import rospy 00005 import smach 00006 00007 from simple_script_server import * 00008 sss = simple_script_server() 00009 00010 from shared_state_information import * 00011 import random 00012 from nav_msgs.msg import Odometry 00013 00014 ## Approach pose state (without retry) 00015 # 00016 # This state tries once to move the robot to the given pose. 00017 # 00018 # Modified to handle user intervention and pre-empty request 00019 ## 00020 class approach_pose_without_retry(smach.State): 00021 00022 def __init__(self, pose = ""): 00023 smach.State.__init__( 00024 self, 00025 outcomes=['succeeded', 'not_completed','failed', 'preempted'], 00026 input_keys=['base_pose']) 00027 00028 self.pose = pose 00029 self.counter =0 00030 self.timeout = 30 00031 self.is_moving = False 00032 self.warnings = ["I can not reach my target position because my path or target is blocked.","My path is blocked.", "I can not reach my target position."] 00033 # Subscriber to base_odometry 00034 rospy.Subscriber("/base_controller/odometry", Odometry, self.callback) 00035 00036 self.mode = "omni" 00037 try: 00038 self.mode = rospy.get_param("srs/common/default_navigation_mode") 00039 except Exception, e: 00040 rospy.loginfo("Parameter Server not ready, use default value for navigation") 00041 00042 00043 #Callback for the /base_controller/odometry subscriber 00044 def callback(self,msg): 00045 r = 0.01 # error range in m/s or rad/s 00046 if (abs(msg.twist.twist.linear.x) > r) or (abs(msg.twist.twist.linear.y) > r) or (abs(msg.twist.twist.angular.z) > r): 00047 self.is_moving = True 00048 else: 00049 self.is_moving = False 00050 return 00051 00052 00053 00054 00055 def execute(self, userdata): 00056 00057 global current_task_info 00058 # robot moved, any previous detection is not valid anymore 00059 current_task_info.set_object_identification_state(False) 00060 00061 if self.preempt_requested(): 00062 self.service_preempt() 00063 return 'preempted' 00064 00065 # determine target position 00066 if self.pose != "": 00067 pose = self.pose 00068 elif type(userdata.base_pose) is str: 00069 pose = userdata.base_pose 00070 elif type(userdata.base_pose) is list: 00071 pose = [] 00072 pose.append(userdata.base_pose[0]) 00073 pose.append(userdata.base_pose[1]) 00074 pose.append(userdata.base_pose[2]) 00075 else: # this should never happen 00076 rospy.logerr("Invalid userdata 'pose'") 00077 return 'failed' 00078 00079 # try reaching pose 00080 handle_base = sss.move("base", pose, mode=self.mode, blocking=False) 00081 #move_second = False 00082 00083 # init variables 00084 stopping_time = 0.0 00085 announce_time = 0.0 00086 freq = 2.0 # Hz 00087 yellow = False 00088 00089 00090 # check for goal status 00091 while not rospy.is_shutdown(): 00092 # stopped or paused due to intervention 00093 if self.preempt_requested(): 00094 self.service_preempt() 00095 #handle_base.set_failed(4) 00096 handle_base.client.cancel_goal() 00097 return 'preempted' 00098 00099 # finished with succeeded 00100 if (handle_base.get_state() == 3): 00101 sss.set_light('green') 00102 return 'succeeded' 00103 # finished with aborted 00104 elif (handle_base.get_state() == 4): 00105 sss.set_light('green') 00106 return 'not_completed' 00107 # finished with preempted or canceled 00108 elif (handle_base.get_state() == 2) or (handle_base.get_state() == 8): 00109 sss.set_light('green') 00110 return 'not_completed' 00111 # return with error 00112 elif (handle_base.get_error_code() > 0): 00113 print "error_code = " + str(handle_base.get_error_code()) 00114 sss.set_light('red') 00115 return 'failed' 00116 00117 # check if the base is moving 00118 loop_rate = rospy.Rate(freq) # hz 00119 if not self.is_moving: # robot stands still 00120 # increase timers 00121 stopping_time += 1.0/freq 00122 announce_time += 1.0/freq 00123 00124 # abort after timeout is reached 00125 if stopping_time >= self.timeout: 00126 sss.stop("base") 00127 sss.set_light('green') 00128 return 'not_completed' 00129 00130 # announce warning after every 10 sec 00131 if announce_time >= 10.0: 00132 sss.say([self.warnings[random.randint(0,len(self.warnings)-1)]],False) 00133 announce_time = 0.0 00134 00135 # set light to "thinking" after not moving for 2 sec 00136 if round(stopping_time) >= 2.0: 00137 sss.set_light("blue") 00138 yellow = False 00139 else: 00140 # robot is moving 00141 if not yellow: 00142 sss.set_light("yellow") 00143 yellow = True 00144 00145 # sleep 00146 loop_rate.sleep()