Go to the documentation of this file.00001
00002 import rospy
00003 import threading
00004 from demo_msgs.msg import *
00005
00006 START = 0
00007 WAIT_FOR_ROBOT = 1
00008 ROBOT_ASSIGNED = 2
00009 ROBOT_NOT_ASSIGNED = 3
00010 ROBOT_READY = 4
00011 ROBOT_NOT_READY = 5
00012 ROBOT_DONE = 6
00013 ROBOT_FAIL = 7
00014
00015 DRINK_READY = 8
00016 DRINK_NOT_READY = 9
00017
00018 class OrderHandler(threading.Thread):
00019
00020 def __init__(self,location,beverage,task_id,pub,done):
00021
00022 threading.Thread.__init__(self)
00023 self.location = location
00024 self.task_id = task_id
00025 self.beverage = beverage
00026 self.status = START
00027 self.front_status = "WAIT"
00028 self.done = done
00029
00030 self.pub = pub
00031
00032 def run(self):
00033 self.log("order has been placed task id = " + str(self.task_id))
00034
00035 while not rospy.is_shutdown() and self.status != ROBOT_ASSIGNED:
00036 self.log("Request Robot")
00037 self.request_robot()
00038 self.wait_for_state(ROBOT_ASSIGNED,ROBOT_NOT_ASSIGNED)
00039 self.log("Robot ready")
00040 self.log("Calling robot to Kitchen")
00041 self.call_robot()
00042 self.log("Waiting for robot to come kitchen")
00043 self.wait_for_state(ROBOT_READY,ROBOT_NOT_READY)
00044
00045
00046 self.log("Waiting for Drink")
00047 self.request_drink()
00048 self.wait_for_state(DRINK_READY,DRINK_NOT_READY)
00049 self.log("Sending robot to the location : " +str(self.location))
00050 self.send_robot()
00051 self.wait_for_state(ROBOT_READY,ROBOT_FAIL)
00052 rospy.sleep(5.0)
00053 self.send_back_to_station()
00054 self.wait_for_state(ROBOT_READY,ROBOT_FAIL)
00055 self.log("Releasing Robot")
00056 rospy.sleep(1.0)
00057 self.release_robot()
00058
00059 if self.status == ROBOT_READY:
00060 self.log("Delivery Status : Success!")
00061 else:
00062 self.log("Delivery Status : Fail!")
00063
00064 self.done(self.task_id)
00065
00066 def request_robot(self):
00067 self.pub['request_robot'].publish(RequestRobot(self.task_id))
00068 self.status = WAIT_FOR_ROBOT
00069
00070 def wait_for_state(self,state_1,state_2):
00071 while not rospy.is_shutdown():
00072 if self.status == state_1 or self.status == state_2:
00073 break
00074 rospy.sleep(1.0)
00075
00076 def call_robot(self):
00077 self.pub['request_goto'].publish(RequestGoto(self.task_id,"kitchen"))
00078 self.status = WAIT_FOR_ROBOT
00079
00080 def request_drink(self):
00081 self.pub['request_drink'].publish(RequestDrink(self.task_id,self.beverage))
00082
00083 def send_back_to_station(self):
00084 self.pub['request_goto'].publish(RequestGoto(self.task_id,"station"))
00085 self.status = WAIT_FOR_ROBOT
00086
00087 def send_robot(self):
00088 self.pub['request_goto'].publish(RequestGoto(self.task_id,self.location))
00089 self.status = WAIT_FOR_ROBOT
00090
00091 def release_robot(self):
00092 self.pub['release_robot'].publish(ReleaseRobot(self.task_id))
00093
00094 def process_response_goto(self,msg):
00095 if msg.status == True:
00096 self.status = ROBOT_READY
00097
00098 def process_response_robot(self,msg):
00099 if msg.ready == True:
00100 self.log("Robot assigned")
00101 self.status = ROBOT_ASSIGNED
00102 else:
00103 self.log("Robot is not assigned")
00104 self.status = ROBOT_NOT_ASSIGNED
00105
00106 def process_response_drink(self,ready):
00107 self.status = DRINK_READY
00108
00109 def log(self,msg):
00110 self.front_status = msg
00111 rospy.loginfo("Kitchen["+str(self.task_id)+"] : " + str(msg))
