map_manager.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2014, Yujin Robot
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # Author: Jorge Santos
00035 # 
00036 # Pythonized version of map_store/map_manager to provide the same services
00037 # within the WCF to keep backward compatibility while client software adapt
00038 # to our new interface.
00039 # Includes also map_store/map_saver functionality but improved according to
00040 # this issue: https://github.com/ros-planning/map_store/issues/4
00041 # 
00042 # behavior:
00043 #  - sets up connection to warehouse
00044 #  - tells warehouse to publish latest map of any session (or default map?  or nothing?)
00045 #  - spins, handling service calls
00046 #  - listens on "map" topic.  On each map:
00047 #     - id_of_most_recent_map = Collection.publish(map, {session ID, map name})
00048 # service calls:
00049 #  - list_maps() returns list of map metadata: {id, name, timestamp, maybe thumbnail}
00050 #    - query for all maps.
00051 #  - delete_map(map id) returns void
00052 #    - Deletes the given map
00053 #  - rename_map(map id, name) returns void
00054 #    - renames a given map
00055 #  - save_map(map name) returns void
00056 #    - save the map returned by dynamic_map as map name
00057 #  - publish_map(map id) returns void
00058 #    - queries warehouse for map of given id
00059 #    - publishes the map on /map topic
00060 #    - sets dynamic map up to load it
00061 #  - dynamic_map() returns nav_msgs/OccupancyGrid
00062 #    - returns the dynamic map
00063 
00064 import rospy
00065 import unique_id
00066 
00067 import warehouse_ros as wr
00068 
00069 from nav_msgs.msg import *
00070 from nav_msgs.srv import *
00071 
00072 from world_canvas_msgs.msg import *
00073 from world_canvas_msgs.srv import *
00074 from world_canvas_utils.serialization import *
00075 
00076 
00077 class MapManager:
00078     
00079     ##########################################################################
00080     # Initialization
00081     ##########################################################################
00082 
00083     def __init__(self):
00084         # Set up map collection
00085         self.map_collection = wr.MessageCollection('world_canvas', 'maps', OccupancyGrid)
00086         self.map_collection.ensure_index('uuid', unique=True)
00087         
00088         # Set up map management services
00089         self.list_maps_srv   = rospy.Service('list_maps',   ListMaps,   self.list_maps)
00090         self.publish_map_srv = rospy.Service('publish_map', PublishMap, self.publish_map)
00091         self.delete_map_srv  = rospy.Service('delete_map',  DeleteMap,  self.delete_map)
00092         self.rename_map_srv  = rospy.Service('rename_map',  RenameMap,  self.rename_map)
00093         self.save_map_srv    = rospy.Service('save_map',    SaveMap,    self.save_map)
00094         self.dynamic_map_srv = rospy.Service('dynamic_map', GetMap,     self.dynamic_map)
00095 
00096         # Set up map subscriber for saving under-construction maps
00097         self.map_subscriber = rospy.Subscriber('map', OccupancyGrid, self.on_map_received, queue_size=1)
00098 
00099         # Use the current ROS time in seconds as the session id for saved maps
00100         self.rec_session = RecSession(self)
00101 
00102         # Set up map publisher and publish the last used map, if any
00103         self.map_publisher = rospy.Publisher('map', OccupancyGrid, latch=True, queue_size=1)
00104 
00105         try:
00106             self.last_map = rospy.get_param('~last_map_id')
00107             map = self.lookup_map(self.last_map)
00108             if map is None:
00109                 rospy.logerr("Invalid last_map_id: %s" % str(self.last_map))
00110             else:
00111                 self.map_publisher.publish(map)
00112         except KeyError:
00113             self.last_map = None
00114 
00115         rospy.loginfo("Map manager : initialized.")
00116 
00117 
00118     ##########################################################################
00119     # Services callbacks
00120     ##########################################################################
00121 
00122     def list_maps(self, request):
00123         rospy.logdebug("Service call : list_maps");
00124         
00125         response = ListMapsResponse()
00126         
00127         all_maps = self.map_collection.query({}, metadata_only=True, sort_by='creation_time', ascending=False)
00128         
00129         # Loop over all maps metadata to get the first of each session.
00130         while True:
00131             try:
00132                 map_md = all_maps.next()
00133                 rospy.logdebug("Add map to result list: %s" % map_md)
00134 
00135                 # Add the map info to our result list.
00136                 new_entry = MapListEntry()
00137                 new_entry.name = map_md.get('name', '')  # name is missing when auto-saving under-construction maps
00138                 new_entry.date = map_md['creation_time']
00139                 new_entry.session_id = map_md['session_id']
00140                 new_entry.map_id = map_md['uuid']
00141                 
00142                 response.map_list.append(new_entry)
00143             except StopIteration:
00144                 break
00145 
00146         return response
00147 
00148 
00149     def lookup_map(self, uuid):
00150         rospy.logdebug("Load map %s" % uuid)
00151         matching_maps = self.map_collection.query({'uuid': {'$in': [uuid]}})
00152         try:
00153             return matching_maps.next()[0]
00154         except StopIteration:
00155             rospy.logerr("No map found for uuid %s" % uuid)
00156             return None
00157 
00158 
00159     def publish_map(self, request):
00160         rospy.logdebug("Service call : publish_map %s" % request.map_id)
00161         response = PublishMapResponse()
00162 
00163         map = self.lookup_map(request.map_id)
00164         if map is None:
00165             rospy.logerr("Invalid map id: %s" % str(request.map_id))
00166             return None
00167         else:
00168             self.last_map = request.map_id
00169             rospy.set_param('~last_map_id', self.last_map)
00170             self.map_publisher.publish(map)
00171 
00172         return response
00173         
00174     def delete_map(self, request):
00175         rospy.logdebug("Service call : delete map %s" % request.map_id)
00176         response = DeleteMapResponse()
00177         
00178         if rospy.has_param('~last_map_id') and rospy.get_param('~last_map_id') == request.map_id:
00179             rospy.delete_param('~last_map_id')
00180         if self.last_map == request.map_id: 
00181             self.last_map = None
00182 
00183         if self.map_collection.remove({'uuid': {'$in': [request.map_id]}}) == 0:
00184             return None
00185 
00186         return response
00187 
00188     def rename_map(self, request):
00189         rospy.logdebug("Service call : rename map %s as %s" % (request.map_id, request.new_name))
00190         response = RenameMapResponse()
00191 
00192         map_metadata = self.get_metadata(request.map_id)
00193         if map_metadata is None:
00194             return None
00195 
00196         map_metadata['name'] = request.new_name
00197         self.map_collection.update(map_metadata)
00198         return response  
00199 
00200     def dynamic_map(self, request):
00201         rospy.logdebug("Service call : get last map (%s)" % self.last_map)
00202         response = GetMapResponse()
00203 
00204         if self.last_map is None:
00205             return None
00206 
00207         map = self.lookup_map(self.last_map)
00208         if map is None:
00209             return None
00210 
00211         response.map = map;
00212         return response
00213 
00214     def on_map_received(self, map_msg):
00215         self.rec_session.map = map_msg
00216 
00217         if not self.rec_session.auto_save:
00218             rospy.logdebug("Map received but auto-save map is off")
00219         else:
00220             self.rec_session.save()
00221 
00222     def save_map(self, request):
00223         rospy.logdebug("Service call : save current map as %s" % request.map_name)
00224         response = SaveMapResponse()
00225 
00226         if self.rec_session.save(request.map_name) == False:
00227             return None
00228 
00229         return response
00230 
00231     def get_metadata(self, uuid):
00232         # Get metadata for the given map id
00233         matching_maps = self.map_collection.query({'uuid': {'$in': [uuid]}}, True)
00234         try:
00235             return matching_maps.next()
00236         except StopIteration:
00237             rospy.logerr("Map %s not found" % uuid)
00238             return None
00239 
00240 
00241 class RecSession:
00242     '''
00243     Keep track of incoming maps (through 'map' topic) so we can save automatically (if parameter
00244     auto_save_map is set as true) or under demand (through save_map service).
00245     To avoid spaming the database as on map_store/map_saver, we use the same metadata (with the
00246     same uuid) so we instead overwrite the current session map with new data.
00247     See https://github.com/ros-planning/map_store/issues/4 for more details.
00248     '''
00249     def __init__(self, parent):
00250         
00251         self.parent = parent
00252         self.map = None
00253         # Generate a unique uuid for the whole session
00254         # No name field, so by default we save maps anonymously
00255         # Use the current ROS time in seconds as the session id for saved maps
00256         self.metadata = {'uuid': str(unique_id.fromRandom()),
00257                          'session_id': str(rospy.get_time())
00258                         }
00259         self.auto_save = rospy.get_param('~auto_save_map', False)
00260         self.map_saved = False
00261 
00262     def save(self, name=None):
00263         if name is not None:
00264             self.metadata['name'] = name
00265 
00266         if self.map is None:
00267             rospy.logerr("No map received so far! Nothing to save")
00268             return False
00269 
00270         if not self.map_saved:
00271             try:
00272                 self.parent.map_collection.insert(self.map, self.metadata, safe=True)
00273                 self.metadata = self.parent.get_metadata(self.metadata['uuid'])
00274                 if self.metadata is None:
00275                     # This should not happen, obviously
00276                     rospy.logerr("Map %s not found just after inserting it???" % request.map_id)
00277                 self.map_saved = True
00278             except Exception as e:
00279                 # Assume collection.insert raised this, as we set safe=True (typically a DuplicateKeyError)
00280                 # This should not happen, as we have generated an uuid; but just in case I copy-paste the code...
00281                 rospy.logerr("Insert map failed: %s" % str(e))
00282                 return False
00283 
00284             rospy.logdebug("Saved map %d by %d @ %f%s" % (self.map.info.width, self.map.info.height,
00285                             self.map.info.resolution, ' as ' + name if name else ''))
00286         else:
00287             self.metadata['creation_time'] = rospy.Time.now().to_sec()
00288             self.parent.map_collection.update(self.metadata, msg=self.map)
00289             rospy.logdebug("Updated map %d by %d @ %f%s" % (self.map.info.width, self.map.info.height,
00290                             self.map.info.resolution, ' as ' + name if name else ''))
00291         return True


world_canvas_server
Author(s): Jorge Santos Simón
autogenerated on Thu Jun 6 2019 21:25:06