├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── map_merging.hpp ├── manifest.xml └── src └── map_merging.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.4.6) 2 | include ($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Initialize the ROS build system. 5 | rosbuild_init () 6 | 7 | # Set the default path for built executables to the "bin" directory. 8 | set (EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 9 | 10 | # Set the name to use for the executable. 11 | set (BINNAME map_merging) 12 | 13 | # Set the source files to use with the executable. 14 | set (SRCS ${SRCS} src/map_merging.cpp) 15 | 16 | # Set the directories where include files can be found. 17 | include_directories (include) 18 | 19 | # Build the executable that will be used to run this node. 20 | rosbuild_add_executable (${BINNAME} ${SRCS}) 21 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Zhi Yan 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # map_merging # 2 | 3 | [http://wiki.ros.org/map_merging](http://wiki.ros.org/map_merging) 4 | -------------------------------------------------------------------------------- /include/map_merging.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Zhi Yan. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Zhi Yan nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef _MAP_MERGING_HPP 38 | #define _MAP_MERGING_HPP 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | static const int UNKNOWN = -1; 48 | static const int OBSTACLE = 100; 49 | static const int FREE = 0; 50 | 51 | struct Pose { 52 | std::string name; 53 | bool received; 54 | geometry_msgs::PoseStamped data; 55 | 56 | Pose() : name(), received(), data() {} 57 | Pose(std::string n, bool r) : name(n), received(r), data() {} 58 | }; 59 | 60 | struct Map { 61 | std::string name; 62 | bool received; 63 | nav_msgs::OccupancyGrid data; 64 | 65 | Map() : name(), received(), data() {} 66 | Map(std::string n, bool r) : name(n), received(r), data() {} 67 | }; 68 | 69 | class MapMerging { 70 | private: 71 | ros::NodeHandle node_; 72 | 73 | /*** ROS parameters ***/ 74 | double merging_rate_; 75 | int max_number_robots_; 76 | double max_comm_distance_; 77 | std::string pose_topic_, map_topic_; 78 | 79 | /*** ROS publishers ***/ 80 | bool *map_has_been_merged_; 81 | nav_msgs::OccupancyGrid merged_map_; 82 | ros::Publisher merged_map_publisher_; 83 | 84 | /*** ROS subsribers ***/ 85 | std::vector poses_; 86 | std::vector pose_subsribers_; 87 | std::vector maps_; 88 | std::vector map_subsribers_; 89 | //boost::mutex pose_mutex_, map_mutex_; 90 | 91 | std::string my_name_, tm_name_; 92 | int my_id_, tm_id_; 93 | 94 | bool poseFound(std::string name, std::vector &poses); 95 | bool mapFound(std::string name, std::vector &maps); 96 | 97 | std::string robotNameParsing(std::string s); 98 | bool getInitPose(std::string name, geometry_msgs::Pose &pose); 99 | bool getRelativePose(std::string n, std::string m, geometry_msgs::Pose &delta, double resolution); 100 | double distBetween(geometry_msgs::Point &p, geometry_msgs::Point &q); 101 | 102 | /*** Map merging algorithms ***/ 103 | void greedyMerging(int delta_x, int delta_y, int map_id); 104 | 105 | public: 106 | MapMerging(); 107 | ~MapMerging(); 108 | 109 | void spin(); 110 | void execute(); 111 | 112 | void topicSubscribing(); 113 | void handShaking(); 114 | void mapMerging(); 115 | 116 | void poseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg, int id); 117 | void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg, int id); 118 | }; 119 | 120 | #endif /* !_MAP_MERGING_HPP */ 121 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Zhi Yan 5 | BSD 6 | 7 | http://www.ai.univ-paris8.fr/~yz/ 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/map_merging.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Zhi Yan. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Zhi Yan nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include "map_merging.hpp" 38 | 39 | MapMerging::MapMerging() { 40 | ros::NodeHandle private_nh("~"); 41 | private_nh.param("merging_rate", merging_rate_, 10.0); 42 | private_nh.param("max_number_robots", max_number_robots_, 100); 43 | private_nh.param("max_comm_distance", max_comm_distance_, 10.0); 44 | private_nh.param("pose_topic", pose_topic_, "pose"); 45 | private_nh.param("map_topic", map_topic_, "map"); 46 | 47 | /* publisher params */ 48 | map_has_been_merged_ = new bool[max_number_robots_](); 49 | merged_map_publisher_ = private_nh.advertise("map", 1, true); 50 | 51 | /* get robot's name */ 52 | my_name_ = ros::this_node::getNamespace(); // Get the robot's name. 53 | my_name_.erase(0, 1); // [ROS_BUG #3671] Remove the first slash in front of the name. 54 | 55 | my_id_ = tm_id_ = UNKNOWN; 56 | } 57 | 58 | MapMerging::~MapMerging() { 59 | delete[] map_has_been_merged_; 60 | } 61 | 62 | /* 63 | * topicSubscribing() 64 | */ 65 | void MapMerging::topicSubscribing() { 66 | ros::master::V_TopicInfo topic_infos; 67 | ros::master::getTopics(topic_infos); 68 | 69 | for(ros::master::V_TopicInfo::const_iterator it_topic = topic_infos.begin(); it_topic != topic_infos.end(); ++it_topic) { 70 | const ros::master::TopicInfo &published_topic = *it_topic; 71 | /* robot pose subscribing */ 72 | if(published_topic.name.find(pose_topic_) != std::string::npos && !poseFound(published_topic.name, poses_)) { 73 | ROS_INFO("[%s]:Subscribe to POSE topic: %s.", (ros::this_node::getName()).c_str(), published_topic.name.c_str()); 74 | poses_.push_back(Pose(published_topic.name, false)); 75 | pose_subsribers_.push_back(node_.subscribe(published_topic.name, 1, boost::bind(&MapMerging::poseCallback, this, _1, poses_.size()-1))); 76 | /* get robot's ID */ 77 | if(my_name_.size() > 0 && published_topic.name.compare(0, my_name_.size(), my_name_) == 0) { 78 | my_id_ = poses_.size()-1; 79 | ROS_INFO("[%s]:My name is %s, ID = %d.", (ros::this_node::getName()).c_str(), my_name_.c_str(), my_id_); 80 | } 81 | } 82 | } 83 | 84 | if(my_id_ == UNKNOWN) { 85 | ROS_WARN("[%s]:Can not get robot's pose.", (ros::this_node::getName()).c_str()); 86 | } 87 | } 88 | 89 | /* 90 | * handShaking(): handshaking if robots are within the communication range. 91 | */ 92 | void MapMerging::handShaking() { 93 | if(my_id_ == UNKNOWN || tm_id_ != UNKNOWN) 94 | return; 95 | 96 | geometry_msgs::Pose my_pose, tm_pose; 97 | if(poses_[my_id_].received && getInitPose(my_name_, my_pose)) { 98 | my_pose.position.x += poses_[my_id_].data.pose.position.x; 99 | my_pose.position.y += poses_[my_id_].data.pose.position.y; 100 | my_pose.position.z += poses_[my_id_].data.pose.position.z; 101 | poses_[my_id_].received = false; 102 | //std::cout << "[" << ros::this_node::getName() << "] " << poses_[my_id_].data.header.frame_id << " x = " << my_pose.position.x << " y = " << my_pose.position.y << std::endl; 103 | 104 | for(int i = 0; i < (int)poses_.size(); i++) { 105 | if(i != my_id_ && !map_has_been_merged_[i]) { 106 | tm_name_ = robotNameParsing(poses_[i].name); 107 | if(poses_[i].received && getInitPose(tm_name_, tm_pose)) { 108 | tm_pose.position.x += poses_[i].data.pose.position.x; 109 | tm_pose.position.y += poses_[i].data.pose.position.y; 110 | tm_pose.position.z += poses_[i].data.pose.position.z; 111 | poses_[i].received = false; 112 | if(distBetween(tm_pose.position, my_pose.position) < max_comm_distance_) { 113 | ROS_DEBUG("[%s]:Handshake with %s.", (ros::this_node::getName()).c_str(), tm_name_.c_str()); 114 | tm_id_ = i; 115 | break; 116 | } 117 | } 118 | } 119 | } 120 | } 121 | 122 | if(tm_id_ == UNKNOWN) { 123 | // do something here ... 124 | } 125 | } 126 | 127 | /* 128 | * mapMerging() 129 | */ 130 | void MapMerging::mapMerging() { 131 | if(my_id_ == UNKNOWN || tm_id_ == UNKNOWN) 132 | return; 133 | 134 | std::string map_topic_name; 135 | for(int i = 0; i < (int)poses_.size(); i++) { 136 | map_topic_name = robotNameParsing(poses_[i].name)+"/"+map_topic_; 137 | if(!mapFound(map_topic_name, maps_)) { 138 | maps_.push_back(Map(map_topic_name, false)); 139 | map_subsribers_.push_back(node_.subscribe(map_topic_name, 1, boost::bind(&MapMerging::mapCallback, this, _1, maps_.size()-1))); 140 | ROS_INFO("[%s]:Subscribe to MAP topic: %s.", (ros::this_node::getName()).c_str(), map_topic_name.c_str()); 141 | } 142 | } 143 | 144 | bool map_merging_end = false; 145 | if(maps_[my_id_].received) { 146 | if(maps_[tm_id_].received) { 147 | ROS_DEBUG("[%s]:Exchange map with %s.", (ros::this_node::getName()).c_str(), tm_name_.c_str()); 148 | geometry_msgs::Pose delta; 149 | if(getRelativePose(my_name_, tm_name_, delta, maps_[my_id_].data.info.resolution)) { 150 | //std::cout << "[" << ros::this_node::getName() << "] deltaX = " << delta.position.x << " deltaY = " << delta.position.y << std::endl; 151 | if(!map_has_been_merged_[my_id_]) { 152 | merged_map_ = maps_[my_id_].data; 153 | map_has_been_merged_[my_id_] = true; 154 | } 155 | greedyMerging(round(delta.position.x), round(delta.position.y), tm_id_); 156 | map_merging_end = true; 157 | } 158 | maps_[tm_id_].received = false; 159 | } 160 | maps_[my_id_].received = false; 161 | } 162 | 163 | if(map_merging_end) { 164 | ROS_DEBUG("[%s]:Map has been merged with %s.", (ros::this_node::getName()).c_str(), tm_name_.c_str()); 165 | map_has_been_merged_[tm_id_] = true; 166 | tm_id_ = UNKNOWN; 167 | 168 | // It has coordinated with all that can be coordinated, so reset. 169 | bool reset_and_publish = true; 170 | for(int i = 0; i < (int)poses_.size(); i++) { 171 | if(!map_has_been_merged_[i]) { 172 | reset_and_publish = false; 173 | break; 174 | } 175 | } 176 | 177 | if(reset_and_publish) { 178 | for(int i = 0; i < (int)poses_.size(); i++) 179 | map_has_been_merged_[i] = false; 180 | merged_map_publisher_.publish(merged_map_); 181 | } 182 | } 183 | } 184 | 185 | /********************* 186 | * callback function * 187 | *********************/ 188 | void MapMerging::poseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg, int id) { 189 | ROS_DEBUG("poseCallback"); 190 | //boost::mutex::scoped_lock pose_lock (pose_mutex_); 191 | poses_[id].received = true; 192 | poses_[id].data = *msg; 193 | } 194 | 195 | void MapMerging::mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg, int id) { 196 | ROS_DEBUG("mapCallback"); 197 | //boost::mutex::scoped_lock map_lock (map_mutex_); 198 | maps_[id].received = true; 199 | maps_[id].data = *msg; 200 | } 201 | 202 | /********************* 203 | * private function * 204 | *********************/ 205 | bool MapMerging::poseFound(std::string name, std::vector &poses) { 206 | for(std::vector::iterator it_pose = poses.begin(); it_pose != poses.end(); ++it_pose) { 207 | if((*it_pose).name.compare(name) == 0) 208 | return true; 209 | } 210 | return false; 211 | } 212 | 213 | bool MapMerging::mapFound(std::string name, std::vector &maps) { 214 | for(std::vector::iterator it_map = maps.begin(); it_map != maps.end(); ++it_map) { 215 | if((*it_map).name.compare(name) == 0) 216 | return true; 217 | } 218 | return false; 219 | } 220 | 221 | std::string MapMerging::robotNameParsing(std::string s) { 222 | return s.erase(s.find('/', 1), s.size()); 223 | } 224 | 225 | /* 226 | * Get robot's initial position 227 | * TODO: get orientation 228 | */ 229 | bool MapMerging::getInitPose(std::string name, geometry_msgs::Pose &pose) { 230 | if(ros::param::get(name+"/map_merging/init_pose_x", pose.position.x) && 231 | ros::param::get(name+"/map_merging/init_pose_y", pose.position.y) && 232 | ros::param::get(name+"/map_merging/init_pose_z", pose.position.z)) { 233 | return true; 234 | } 235 | return false; 236 | } 237 | 238 | /* 239 | * Get the relative position of two robots 240 | * TODO: get orientation 241 | */ 242 | bool MapMerging::getRelativePose(std::string n, std::string m, geometry_msgs::Pose &delta, double resolution) { 243 | geometry_msgs::Pose p, q; 244 | if(ros::param::get(n+"/map_merging/init_pose_x", p.position.x) && 245 | ros::param::get(n+"/map_merging/init_pose_y", p.position.y) && 246 | ros::param::get(n+"/map_merging/init_pose_z", p.position.z) && 247 | ros::param::get(m+"/map_merging/init_pose_x", q.position.x) && 248 | ros::param::get(m+"/map_merging/init_pose_y", q.position.y) && 249 | ros::param::get(m+"/map_merging/init_pose_z", q.position.z)) { 250 | delta.position.x = round((p.position.x - q.position.x) / resolution); 251 | delta.position.y = round((p.position.y - q.position.y) / resolution); 252 | delta.position.z = round((p.position.z - q.position.z) / resolution); 253 | return true; 254 | } 255 | return false; 256 | } 257 | 258 | double MapMerging::distBetween(geometry_msgs::Point &p, geometry_msgs::Point &q) { 259 | // Euclidean distance 260 | return sqrt((p.x-q.x)*(p.x-q.x)+(p.y-q.y)*(p.y-q.y)+(p.z-q.z)*(p.z-q.z)); 261 | } 262 | 263 | /* 264 | * Algorithm 1 - Greedy Merging 265 | * yz14simpar 266 | */ 267 | void MapMerging::greedyMerging(int delta_x, int delta_y, int map_id) { 268 | for(int i = 0; i < (int)merged_map_.info.width; i++) { 269 | for(int j = 0; j < (int)merged_map_.info.height; j++) { 270 | if(i+delta_x >= 0 && i+delta_x < (int)maps_[map_id].data.info.width && 271 | j+delta_y >= 0 && j+delta_y < (int)maps_[map_id].data.info.height) { 272 | if((int)merged_map_.data[i+j*(int)merged_map_.info.width] == UNKNOWN) 273 | merged_map_.data[i+j*(int)merged_map_.info.width] = maps_[map_id].data.data[i+delta_x+(j+delta_y)*(int)maps_[map_id].data.info.width]; 274 | } 275 | } 276 | } 277 | } 278 | 279 | /* 280 | * execute() 281 | */ 282 | void MapMerging::execute() { 283 | ros::Rate r(merging_rate_); 284 | while(node_.ok()) { 285 | topicSubscribing(); 286 | handShaking(); 287 | mapMerging(); 288 | r.sleep(); 289 | } 290 | } 291 | 292 | /* 293 | * spin() 294 | */ 295 | void MapMerging::spin() { 296 | ros::spinOnce(); 297 | boost::thread t(boost::bind(&MapMerging::execute, this)); 298 | ros::spin(); 299 | t.join(); 300 | } 301 | 302 | int main(int argc, char **argv) { 303 | ros::init(argc, argv, "map_merging"); 304 | MapMerging map_merging; 305 | map_merging.spin(); 306 | return 0; 307 | } 308 | --------------------------------------------------------------------------------