Skip to content

Commit 0157dbd

Browse files
author
yzrobot
committed
new file: CMakeLists.txt
 new file: include/map_merging.hpp new file: manifest.xml new file: src/map_merging.cpp
0 parents  commit 0157dbd

File tree

4 files changed

+460
-0
lines changed

4 files changed

+460
-0
lines changed

‎CMakeLists.txt

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
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})

‎include/map_merging.hpp

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
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 <ros/ros.h>
41+
#include <geometry_msgs/PoseStamped.h>
42+
#include <nav_msgs/OccupancyGrid.h>
43+
44+
#include <boost/thread.hpp>
45+
#include <boost/bind.hpp>
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<Pose> poses_;
86+
std::vector<ros::Subscriber> pose_subsribers_;
87+
std::vector<Map> maps_;
88+
std::vector<ros::Subscriber> 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<Pose> &poses);
95+
bool mapFound(std::string name, std::vector<Map> &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 */

‎manifest.xml

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<package>
2+
<description brief="map_merging">
3+
</description>
4+
<author>Zhi Yan</author>
5+
<license>BSD</license>
6+
<review status="unreviewed" notes=""/>
7+
<url>http://www.ai.univ-paris8.fr/~yz/</url>
8+
9+
<depend package="roscpp"/>
10+
<depend package="geometry_msgs"/>
11+
<depend package="nav_msgs"/>
12+
<depend package="tf"/>
13+
</package>

0 commit comments

Comments
 (0)