$search
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 }