1 #!/usr/bin/env python
   2 #
   3 # License: BSD
   4 #   https://raw.github.com/robotics-in-concert/rocon_services/license/LICENSE
   5 #
   6 ##############################################################################
   7 # About
   8 ##############################################################################
   9 
  10 # Simple script to request a pre-configured number of strolling turtles.
  11 # This could also be done with link graphs, but chatter concert does that.
  12 
  13 ##############################################################################
  14 # Imports
  15 ##############################################################################
  16 
  17 import rospy
  18 import rocon_python_comms
  19 import concert_service_utilities
  20 import rocon_scheduler_requests
  21 import unique_id
  22 import scheduler_msgs.msg as scheduler_msgs
  23 
  24 ##############################################################################
  25 # Classes
  26 ##############################################################################
  27 
  28 
  29 class TurtlePond:
  30     '''
  31       Requets handling for getting its mittens on some turtle_stroll'ing
  32       turtles.
  33     '''
  34     __slots__ = [
  35         'service_name',
  36         'service_description',
  37         'service_id',
  38         'requester',
  39         'pending_requests',   # [uuid.UUID] list of request id's pending feedback from the scheduler
  40         'allocated_requests'  # [uuid.UUID] moved here from pending requests once granted
  41     ]
  42 
  43     def __init__(self):
  44         ####################
  45         # Discovery
  46         ####################
  47         (self.service_name, self.service_description, self.service_id) = concert_service_utilities.get_service_info()
  48 
  49         ####################
  50         # Setup
  51         ####################
  52         self.requester = self.setup_requester(self.service_id)
  53         self.pending_requests = []
  54         self.allocated_requests = []
  55         number_of_turtles = rospy.get_param("turtles", default=1)
  56         rospy.loginfo("TurtlePond : requesting %s turtles" % number_of_turtles)
  57         for unused_i in range(0, number_of_turtles):
  58             self.request_turtles()
  59 
  60     def setup_requester(self, uuid):
  61         try:
  62             scheduler_requests_topic_name = concert_service_utilities.find_scheduler_requests_topic()
  63             #rospy.loginfo("Service : found scheduler [%s][%s]" % (topic_name))
  64         except rocon_python_comms.NotFoundException as e:
  65             rospy.logerr("TurtlePond : %s" % (str(e)))
  66             return  # raise an exception here?
  67         frequency = rocon_scheduler_requests.common.HEARTBEAT_HZ
  68         return rocon_scheduler_requests.Requester(self.requester_feedback, uuid, 0, scheduler_requests_topic_name, frequency)
  69 
  70     def request_turtles(self):
  71         '''
  72          Request a turtle.
  73         '''
  74         resource = scheduler_msgs.Resource()
  75         resource.id = unique_id.toMsg(unique_id.fromRandom())
  76         resource.rapp = 'turtle_concert/turtle_stroll'
  77         resource.uri = 'rocon:/'
  78         resource_request_id = self.requester.new_request([resource])
  79         self.pending_requests.append(resource_request_id)
  80         self.requester.send_requests()
  81 
  82     def requester_feedback(self, request_set):
  83         '''
  84           Keep an eye on our pending requests and see if they get allocated here.
  85           Once they do, kick them out of the pending requests list so _ros_capture_teleop_callback
  86           can process and reply to the interaction.
  87 
  88           @param request_set : the modified requests
  89           @type dic { uuid.UUID : scheduler_msgs.ResourceRequest }
  90         '''
  91         for request_id, request in request_set.requests.iteritems():
  92             if request_id in self.pending_requests:
  93                 if request.msg.status == scheduler_msgs.Request.GRANTED:
  94                     self.pending_requests.remove(request_id)
  95                     self.allocated_requests.append(request_id)
  96         # Need to add logic here for when the request gets released or preempted
  97 
  98     def cancel_all_requests(self):
  99         '''
 100           Exactly as it says! Used typically when shutting down or when
 101           it's lost more allocated resources than the minimum required (in which case it
 102           cancels everything and starts reissuing new requests).
 103         '''
 104         self.requester.cancel_all()
 105         self.requester.send_requests()
 106 
 107 ##############################################################################
 108 # Launch point
 109 ##############################################################################
 110 
 111 if __name__ == '__main__':
 112 
 113     rospy.init_node('turtle_pond')
 114     turtle_pond = TurtlePond()
 115     rospy.spin()
 116     if not rospy.is_shutdown():
 117         turtle_pond.cancel_all_requests()