Go to the documentation of this file.00001
00002 import roslib; roslib.load_manifest('multi_resource_handler')
00003 import rospy
00004 from demo_msgs.msg import *
00005 from .turtle import *
00006
00007 class MultiTurtlebotHandler(object):
00008
00009 def __init__(self):
00010 self.sub = {}
00011 self.sub['request_robot'] = rospy.Subscriber('/mrh/request_robot',RequestRobot,self.process_request_robot)
00012 self.sub['request_goto'] = rospy.Subscriber('/mrh/request_goto',RequestGoto,self.process_request_goto)
00013 self.sub['release_robot'] =rospy.Subscriber('/mrh/release_robot',ReleaseRobot,self.process_release_robot)
00014
00015 self.pub = {}
00016 self.pub['response_robot'] = rospy.Publisher('/mrh/response_robot',ResponseRobot)
00017 self.pub['response_goto'] = rospy.Publisher('/mrh/response_goto',ResponseGoto)
00018
00019 self.turtle = {}
00020
00021 self.turtle['bach'] = Turtle('turtle1',False)
00022 self.turtle['chopin'] = Turtle('turtle2',False)
00023
00024 self.task = {}
00025
00026 self.location = {}
00027 self.location['station'] = rospy.get_param('~station',[0, 0])
00028 self.location['kitchen'] = rospy.get_param('~kitchen',[-1.1724, 2.3408])
00029 self.location['customer1'] = rospy.get_param('~customer1',[-3.60909,-0.09575])
00030 self.location['customer2'] = rospy.get_param('~customer2',[-2.9145,1.6521])
00031
00032 def spin(self):
00033 rospy.spin()
00034
00035 def process_request_robot(self,msg):
00036 self.log("Robot requested")
00037
00038 for i,t in self.turtle.items():
00039 if t.status == False:
00040 t.set_task(msg.task_id)
00041 self.task[msg.task_id] = t
00042 self.pub['response_robot'].publish(ResponseRobot(msg.task_id,True))
00043 self.log(str(t.name) + " is assigned for " + str(msg.task_id))
00044 return
00045
00046 self.pub['response_robot'].publish(ResponseRobot(msg.task_id,False))
00047
00048
00049 def process_request_goto(self,msg):
00050
00051 location = None
00052 if msg.location not in self.location:
00053 location = self.location['customer1']
00054 else:
00055 location = self.location[msg.location]
00056 self.log("Request move the robot to " + str(location))
00057
00058 self.task[msg.task_id].goto(location)
00059
00060 self.log("Robot has arrived to "+ str(msg.location))
00061 self.pub['response_goto'].publish(ResponseGoto(msg.task_id,True))
00062
00063 def process_release_robot(self,msg):
00064 self.task[msg.task_id].release()
00065 self.task[msg.task_id] = None
00066
00067 def process_response_move_robot(self,msg):
00068 self.task[msg.task_id].process_response_move_robot(msg)
00069
00070 def log(self,msg):
00071 rospy.loginfo("Multi Handler : " + str(msg))
