make_a_map_pimp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # License: BSD
00004 #   https://raw.github.com/robotics-in-concert/concert_services/license/LICENSE
00005 #
00006 ##############################################################################
00007 # About
00008 ##############################################################################
00009 
00010 # Simple script to pimp out make a map operations for rocon interactions.
00011 #
00012 # - watch the app manager status and when it has a remote controller,
00013 # - flip a spawn/kill pair across
00014 # - call the spawn api
00015 #  - the turtle herder will flip back some handles then.
00016 
00017 ##############################################################################
00018 # Imports
00019 ##############################################################################
00020 
00021 import rospy
00022 import rocon_uri
00023 import concert_service_utilities
00024 import unique_id
00025 import rocon_std_msgs.msg as rocon_std_msgs
00026 import scheduler_msgs.msg as scheduler_msgs
00027 import concert_service_msgs.msg as concert_service_msgs
00028 
00029 class MakeAMapPimp(concert_service_utilities.ResourcePimp):
00030 
00031     _default_cmd_vel_topic = '/teleop/cmd_vel'
00032     _default_compressed_image_topic = '/teleop/compressed_image'
00033     _default_map_topic = 'map'
00034     _default_scan_topic = '/make_a_map/scan'
00035     _default_robot_pose_topic = 'robot_pose'
00036     _default_wc_namespace = 'world_canvas'
00037 
00038     def setup_variables(self):
00039         '''
00040             Need to setup the following variables
00041             service_priority, service_id, resource_type, available_resource_publisher_name, capture_topic_name
00042         '''
00043         (service_name, service_description, service_priority, service_id) = concert_service_utilities.get_service_info()
00044         self.service_priority = service_priority
00045         self.service_id = service_id
00046         self.resource_type = 'turtlebot_rapps/make_a_map'
00047         self.available_resource_publisher_name = 'available_make_a_map'
00048         self.capture_topic_name = 'capture_make_a_map'
00049 
00050     def ros_capture_callback(self, request_id, msg):
00051         '''
00052          Processes the service pair server 'capture_teleop'. This will run
00053          in a thread of its own for each request. It has a significantly long lock
00054          though - this needs to get fixed.
00055         '''
00056         # Todo : request the scheduler for this resource,
00057         # use self.allocation_timeout to fail gracefully
00058 
00059         response = concert_service_msgs.CaptureResourceResponse()
00060         response.result = False
00061         if not msg.release:  # i.e. we're capturing:
00062             if msg.rocon_uri not in [r.uri for r in self.available_resources]:
00063                 self.logwarn("couldn't capture resource [not available][%s]" % msg.rocon_uri)
00064                 response.result = False
00065             else:
00066                 resource = self._create_resource(msg.rocon_uri)
00067                 request_result, resource_request_id = self.send_allocation_request(resource)
00068                 response.result = request_result
00069                 if request_result == False:
00070                     self.logwarn("couldn't capture resource [timed out][%s]" % msg.rocon_uri)
00071                 else:
00072                     self.loginfo("captured resource [%s][%s]" % (msg.rocon_uri, resource_request_id))
00073                     response.remappings = resource.remappings
00074         else:  # we're releasing
00075             self.send_releasing_request(msg.rocon_uri)
00076             response.result = True
00077         return response
00078 
00079     def _create_resource(self, uri):
00080         # Create a resource to request
00081         resource = scheduler_msgs.Resource()
00082         resource.id = unique_id.toMsg(unique_id.fromRandom())
00083         resource.rapp = self.resource_type
00084         resource.uri = uri
00085         cmd_vel_remapped, compressed_image_topic_remapped, map_remapped, scan_remapped, robot_pose_remapped = self._get_remapped_topic(rocon_uri.parse(resource.uri).name.string)
00086         resource.remappings = [rocon_std_msgs.Remapping(self._default_cmd_vel_topic, cmd_vel_remapped), rocon_std_msgs.Remapping(self._default_compressed_image_topic, compressed_image_topic_remapped), rocon_std_msgs.Remapping(self._default_map_topic, map_remapped), rocon_std_msgs.Remapping(self._default_scan_topic, scan_remapped), rocon_std_msgs.Remapping(self._default_robot_pose_topic, robot_pose_remapped)]
00087         return resource
00088 
00089     def _get_remapped_topic(self, name):
00090         '''
00091           Sets up remapping rules for Rapp configuration
00092         '''
00093         cmd_vel_remapped = '/' + name + self._default_cmd_vel_topic
00094         compressed_image_topic_remapped = '/' + name + self._default_compressed_image_topic
00095         map_remapped = rospy.get_param('map_topic','/map')
00096         scan_remapped = '/' + name + self._default_scan_topic
00097         robot_pose_remapped = '/' + name + '/' + self._default_robot_pose_topic
00098 
00099         return cmd_vel_remapped, compressed_image_topic_remapped,map_remapped, scan_remapped, robot_pose_remapped
00100 
00101     def loginfo(self, msg):
00102         rospy.loginfo("MakeAMapPimp : %s"%str(msg))
00103 
00104     def logwarn(self, msg):
00105         rospy.logwarn("MakeAMapPimp : %s"%str(msg))
00106 
00107     def logerr(self, msg):
00108         rospy.logerr("MakeAMapPimp : %s"%str(msg))
00109 
00110 
00111 ##############################################################################
00112 # Launch point
00113 ##############################################################################
00114 
00115 if __name__ == '__main__':
00116     rospy.init_node('make_a_map_pimp')
00117     pimp = MakeAMapPimp()
00118     rospy.spin()
00119     if not rospy.is_shutdown():
00120         pimp.cancel_all_requests()


concert_service_initialisation
Author(s): Jihoon Lee
autogenerated on Wed Nov 19 2014 12:37:04