map_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031 behavior:
00032  - sets up connection to warehouse
00033  - tells warehouse to publish latest map of any session (or default map?  or nothing?)
00034  - spins, handling service calls
00035 
00036 service calls:
00037  - list_maps() returns list of map metadata: {id, name, timestamp, maybe thumbnail}
00038    - query for all maps.
00039  - delete_map(map id) returns void
00040    - Deletes the given map
00041  - rename_map[(map id, name) returns void
00042    - renames a given map
00043  - publish_map(map id) returns void
00044    - queries warehouse for map of given id
00045    - publishes the map on /map
00046    - sets dynamic map up to load it\
00047  - dynamic_map() returns nav_msgs/OccupancyGrid
00048    - returns the dynamic map
00049  */
00050 
00051 #include <mongo_ros/message_collection.h>
00052 #include <ros/ros.h>
00053 #include <nav_msgs/OccupancyGrid.h>
00054 #include <map_store/ListMaps.h>
00055 #include <map_store/PublishMap.h>
00056 #include <map_store/DeleteMap.h>
00057 #include <map_store/RenameMap.h>
00058 #include <map_store/MapListEntry.h>
00059 #include <nav_msgs/GetMap.h>
00060 
00061 #include <string>
00062 #include <sstream>
00063 #include <exception>
00064 namespace mr=mongo_ros;
00065 
00066 mr::MessageCollection<nav_msgs::OccupancyGrid> *map_collection;
00067 ros::Publisher map_publisher;
00068 std::string last_map;
00069 
00070 typedef std::vector<mr::MessageWithMetadata<nav_msgs::OccupancyGrid>::ConstPtr> MapVector;
00071 
00072 bool listMaps(map_store::ListMaps::Request &request,
00073                   map_store::ListMaps::Response &response)
00074 {
00075   ROS_DEBUG("listMaps() service call");
00076 
00077   MapVector all_maps;
00078   all_maps = map_collection->pullAllResults( mr::Query(), true, "creation_time", false );
00079 
00080   // Loop over all_maps to get the first of each session.
00081   for(MapVector::const_iterator map_iter = all_maps.begin(); map_iter != all_maps.end(); map_iter++)
00082   {
00083     ROS_DEBUG("listMaps() reading a map");
00084 
00085     ROS_DEBUG("listMaps() adding a map to the result list.");
00086     ROS_DEBUG("listMaps() metadata is: '%s'", (*map_iter)->metadata.toString().c_str());
00087     
00088     // Add the map info to our result list.
00089     map_store::MapListEntry new_entry;
00090     new_entry.name = (*map_iter)->lookupString("name");
00091     new_entry.date = (int64_t)(*map_iter)->lookupDouble("creation_time");
00092     new_entry.session_id = (*map_iter)->lookupString("session_id");
00093     new_entry.map_id = (*map_iter)->lookupString("uuid");
00094     
00095     response.map_list.push_back(new_entry);
00096   }
00097 
00098   ROS_DEBUG("listMaps() service call done");
00099   return true;
00100 }
00101 
00102 bool lookupMap(std::string name, nav_msgs::OccupancyGridConstPtr &ptr) {
00103   MapVector matching_maps;
00104   try
00105   {
00106     matching_maps = map_collection->pullAllResults(mr::Query("uuid", name), false );
00107   } catch(const std::exception &e) {
00108     ROS_ERROR("Error during query: %s", e.what());
00109     return false;
00110   }
00111 
00112   if (matching_maps.size() != 1)
00113   {
00114     ROS_ERROR("publishMap() found %d matching maps instead of 1.  Failing.", (int) matching_maps.size());
00115     return false;
00116   }
00117   ptr = nav_msgs::OccupancyGridConstPtr( matching_maps[0] );
00118   return true;
00119 }
00120 
00121 bool publishMap(map_store::PublishMap::Request &request,
00122                 map_store::PublishMap::Response &response)
00123 {
00124   ROS_DEBUG("publishMap() service call");
00125   ROS_DEBUG("Searching for '%s'", request.map_id.c_str());
00126 
00127   last_map = request.map_id;
00128   ros::NodeHandle nh;
00129   nh.setParam("last_map_id", last_map);
00130   nav_msgs::OccupancyGridConstPtr map;
00131   if (lookupMap(request.map_id, map))
00132   {
00133     try {
00134       map_publisher.publish(map);
00135     } catch(...) {
00136       ROS_ERROR("Error publishing map");
00137     }
00138   }
00139   else
00140   {
00141     return false;
00142   }
00143 
00144   return true;
00145 }
00146 
00147 bool deleteMap(map_store::DeleteMap::Request &request,
00148                map_store::DeleteMap::Response &response)
00149 {
00150   ros::NodeHandle nh;
00151   std::string param;
00152   if (nh.getParam("last_map_id", param))
00153   {
00154     if (param == request.map_id)
00155     {
00156       nh.deleteParam("last_map_id");
00157     }
00158   }
00159   if (last_map == request.map_id) 
00160   {
00161     last_map = "";
00162   }
00163   return map_collection->removeMessages(mr::Query("uuid", request.map_id)) == 1;
00164 }
00165 
00166 bool renameMap(map_store::RenameMap::Request &request,
00167                map_store::RenameMap::Response &response)
00168 {
00169   map_collection->modifyMetadata(mr::Query("uuid", request.map_id), mr::Metadata("name", request.new_name));
00170   return true;
00171 }
00172 
00173 
00174 bool dynamicMap(nav_msgs::GetMap::Request &request,
00175                 nav_msgs::GetMap::Response &response) {
00176   if (last_map == "") {
00177     return false;
00178   }
00179   nav_msgs::OccupancyGridConstPtr map;
00180   if (lookupMap(last_map, map))
00181   {
00182     response.map = *map;
00183   }
00184   else 
00185   {
00186     return false;
00187   }  
00188   return true;
00189 }
00190 
00191 int main (int argc, char** argv)
00192 {
00193   ros::init(argc, argv, "map_manager");
00194   ros::NodeHandle nh;
00195 
00196   map_collection = new mr::MessageCollection<nav_msgs::OccupancyGrid>("map_store", "maps");
00197   map_collection->ensureIndex("uuid");
00198 
00199   if (!nh.getParam("last_map_id", last_map))
00200   {
00201     last_map = "";
00202   }
00203 
00204 
00205   map_publisher = nh.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00206   if (last_map != "")
00207   {
00208     nav_msgs::OccupancyGridConstPtr map;
00209     if (lookupMap(last_map, map))
00210     {
00211       try {
00212         map_publisher.publish(map);
00213       } catch(...) {
00214         ROS_ERROR("Error publishing map");
00215       }
00216     }
00217     else
00218     {
00219       ROS_ERROR("Invalid last_map_id");
00220     }
00221   }
00222 
00223   ros::ServiceServer list_maps_service = nh.advertiseService("list_maps", listMaps);
00224   ros::ServiceServer publish_map_service = nh.advertiseService("publish_map", publishMap);
00225   ros::ServiceServer delete_map_service = nh.advertiseService("delete_map", deleteMap);
00226   ros::ServiceServer rename_map_service = nh.advertiseService("rename_map", renameMap);
00227   ros::ServiceServer dynamic_map = nh.advertiseService("dynamic_map", dynamicMap);
00228 
00229   ROS_DEBUG("spinning.");
00230 
00231   ros::spin();
00232 
00233   delete map_collection;
00234 
00235   return 0;
00236 }


map_store
Author(s): Dave Hershberger
autogenerated on Wed Aug 26 2015 12:28:59