$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 - listens on "map" topic. On each map: 00033 - id_of_most_recent_map = Collection.publish(map, {session ID, map name}) 00034 00035 service calls: 00036 - save_map(map name) returns void 00037 - save the map returned by dynamic_map as map name 00038 */ 00039 00040 #include <mongo_ros/message_collection.h> 00041 #include <ros/ros.h> 00042 #include <nav_msgs/OccupancyGrid.h> 00043 #include <nav_msgs/GetMap.h> 00044 #include <map_store/SaveMap.h> 00045 00046 #include <string> 00047 #include <sstream> 00048 00049 #include <uuid/uuid.h> 00050 00051 namespace mr=mongo_ros; 00052 00053 std::string session_id; 00054 mr::MessageCollection<nav_msgs::OccupancyGrid> *map_collection; 00055 ros::ServiceClient add_metadata_service_client; 00056 ros::ServiceClient dynamic_map_service_client; 00057 00058 std::string uuidGenerate() { 00059 uuid_t uuid; 00060 uuid_generate(uuid); 00061 char uuid_string[37]; // UUID prints into 36 bytes + NULL terminator 00062 uuid_unparse_lower(uuid, uuid_string); 00063 return std::string(uuid_string); 00064 } 00065 00066 void onMapReceived(const nav_msgs::OccupancyGrid::ConstPtr& map_msg) 00067 { 00068 ROS_DEBUG("received map"); 00069 std::string uuid_string = uuidGenerate(); 00070 mr::Metadata metadata 00071 = mr::Metadata("uuid", uuid_string, 00072 "session_id", session_id); 00073 00074 map_collection->insert(*map_msg, metadata); 00075 00076 ROS_DEBUG("saved map"); 00077 } 00078 00079 bool saveMap(map_store::SaveMap::Request &req, 00080 map_store::SaveMap::Response &res) 00081 { 00082 nav_msgs::GetMap srv; 00083 if (!dynamic_map_service_client.call(srv)) { 00084 ROS_ERROR("Dynamic map getter service call failed"); 00085 return false; 00086 } 00087 00088 std::string uuid_string = uuidGenerate(); 00089 mr::Metadata metadata 00090 = mr::Metadata("uuid", uuid_string, 00091 "session_id", session_id, 00092 "name", req.map_name); 00093 00094 ROS_DEBUG("Save map %d by %d @ %f as %s", srv.response.map.info.width, 00095 srv.response.map.info.height, srv.response.map.info.resolution, req.map_name.c_str()); 00096 map_collection->insert(srv.response.map, metadata); 00097 00098 ROS_DEBUG("nameLastestMaps() service call done"); 00099 return true; 00100 } 00101 00102 int main (int argc, char** argv) 00103 { 00104 ros::init(argc, argv, "map_saver"); 00105 ros::NodeHandle nh; 00106 00107 // Use the current ROS time in seconds as the session id. 00108 char buff[256]; 00109 snprintf(buff, 256, "%f", ros::Time::now().toSec()); 00110 session_id = std::string(buff); 00111 00112 map_collection = new mr::MessageCollection<nav_msgs::OccupancyGrid>("map_store", "maps"); 00113 00114 ros::Subscriber map_subscriber = nh.subscribe("map", 1, onMapReceived); 00115 00116 ros::ServiceServer name_latest_map_service = nh.advertiseService("save_map", saveMap); 00117 dynamic_map_service_client = nh.serviceClient<nav_msgs::GetMap>("dynamic_map"); 00118 00119 ROS_DEBUG("spinning."); 00120 00121 ros::spin(); 00122 return 0; 00123 }