├── CMakeLists.txt
├── README.md
├── costmap_plugins.xml
├── include
    └── multibot_layers
    │   └── multibot_layer.h
├── package.xml
└── src
    └── multibot_layer.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(multibot_layers)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   costmap_2d
 6 |   dynamic_reconfigure
 7 |   roscpp
 8 | )
 9 | 
10 | catkin_package(
11 |   INCLUDE_DIRS include
12 |   LIBRARIES multibot_layers
13 |   CATKIN_DEPENDS costmap_2d dynamic_reconfigure roscpp
14 | )
15 | 
16 | include_directories(
17 |   include
18 |   ${catkin_INCLUDE_DIRS}
19 | )
20 | 
21 | add_library(multibot_layer src/multibot_layer.cpp)
22 | 
23 | 
24 | install(FILES costmap_plugins.xml
25 | 	DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
26 | 
27 | install(TARGETS multibot_layer
28 | 	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
29 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | ## Multibot_layers
 2 | A costmap layer allows robot to communicate and share location to each others. Through the ROS parameter server, robots publish their pose to the corresponding rosparam under its own name prefix, for example, /omni_1/pose_x and /omni_1/pose_y, when the costmap updates.
 3 | 
 4 | ## Implementation
 5 | Before diving into this repo, read this [wonderful tutorial](http://wiki.ros.org/costmap_2d/Tutorials/Creating%20a%20New%20Layer). The differences between the multibot_layers and the simple_layers represented in the ROS Wiki tutorial are changes in updateBounds() and updateCosts() functions.
 6 | * ***updateBounds()*** does not change the costmap. It just defines the area that will need to be updated. It first publish its own pose to the parameter server under the specific prefix. And then We calculate the point we want to change in mark[][] by checking the poses of other robots on parameter server. Finally, we expand the min/max bounds to be sure it includes the new point. 
 7 | * ***updateCosts()*** calculates which grid cell our point is in using worldToMap. Then we set the cost of that cell.
 8 | 
 9 | ### pseudocode
10 | ```
11 | # in updateBounds
12 | 
13 | set param -- /omni_num/pose_x -- to its pose_x relative to "map" frame
14 | set param -- /omni_num/pose_x -- to its pose_x relative to "map" frame
15 | find its own prefix by "searchParam"
16 | for i (0 to 4) do 
17 |     save the poses of omni_i, except itself, relative to "map" frame map[i]
18 | end for
19 | ```
20 | p s. [searchParam](http://wiki.ros.org/roscpp/Overview/Parameter%20Server) helps you to get a parameter from the closest namespace.  
21 | ```
22 | # in updateCosts
23 | 
24 | for i (0 to 4) do
25 |     set Lethal_obstacle to the poses occupied by other robots.
26 | end for
27 | ```
28 | 
29 | ## Usage
30 | 
31 | ### Clone the pkg
32 | ```
33 | cd ~/catkin_ws/src
34 | git clone https://github.com/airuchen/multibot_layer.git 
35 | cd ~/catkin_ws
36 | rosdep install --from-paths src --ignore-src -r -y
37 | catkin_make
38 | source devel/setup.bash
39 | ```
40 | ### Add the layer into the costmap yaml file
41 | ```
42 | plugins:
43 | - {name: multibot_layer,     type: "multibot_layer_namespace::MultibotLayer"}
44 | 
45 | ```
46 | 
47 | ## Try it yourself
48 | Follow the tutorial [here](https://github.com/airuchen/turtlebot3) to try out this plugin with multiple turtlebot3.


--------------------------------------------------------------------------------
/costmap_plugins.xml:
--------------------------------------------------------------------------------
1 | <class_libraries>
2 |   <library path="lib/libmultibot_layer">
3 |     <class type="multibot_layer_namespace::MultibotLayer" base_class_type="costmap_2d::Layer">
4 |     </class>
5 |   </library>
6 | </class_libraries>
7 | 


--------------------------------------------------------------------------------
/include/multibot_layers/multibot_layer.h:
--------------------------------------------------------------------------------
 1 | #ifndef MULTIBOT_LAYER_H_
 2 | #define MULTIBOT_LAYER_H_
 3 | #include <ros/ros.h>
 4 | #include <costmap_2d/layer.h>
 5 | #include <costmap_2d/layered_costmap.h>
 6 | #include <costmap_2d/GenericPluginConfig.h>
 7 | #include <dynamic_reconfigure/server.h>
 8 |  
 9 | namespace multibot_layer_namespace
10 | {
11 | 
12 | class MultibotLayer : public costmap_2d::Layer
13 | {
14 | public:
15 |   MultibotLayer();
16 |  
17 |   virtual void onInitialize();
18 |   virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x,
19 |                              double* max_y);
20 |   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
21 |  
22 | private:
23 |   void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
24 | 
25 |   
26 |   double mark[10][2];
27 |   ros::NodeHandle nh;
28 |   dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
29 | };
30 | }
31 | #endif
32 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>multibot_layers</name>
 4 |   <version>1.0.0</version>
 5 |   <description>The multibot_layers package</description>
 6 | 
 7 |   <maintainer email="b10307115@gapps.ntust.edu.tw">airuchen</maintainer>
 8 | 
 9 | 
10 |   <license>Apache 2.0</license>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 |   <build_depend>costmap_2d</build_depend>
14 |   <build_depend>dynamic_reconfigure</build_depend>
15 |   <build_depend>roscpp</build_depend>
16 |   <build_export_depend>costmap_2d</build_export_depend>
17 |   <build_export_depend>dynamic_reconfigure</build_export_depend>
18 |   <build_export_depend>roscpp</build_export_depend>
19 |   <exec_depend>costmap_2d</exec_depend>
20 |   <exec_depend>dynamic_reconfigure</exec_depend>
21 |   <exec_depend>roscpp</exec_depend>
22 | 
23 | 
24 |   <export>
25 |     <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
26 |   </export>
27 | </package>
28 | 


--------------------------------------------------------------------------------
/src/multibot_layer.cpp:
--------------------------------------------------------------------------------
 1 | #include<multibot_layers/multibot_layer.h>
 2 | #include <pluginlib/class_list_macros.h>
 3 | 
 4 | PLUGINLIB_EXPORT_CLASS(multibot_layer_namespace::MultibotLayer, costmap_2d::Layer)
 5 | 
 6 | 	using costmap_2d::LETHAL_OBSTACLE;
 7 | 
 8 | 	namespace multibot_layer_namespace
 9 | {
10 | 	MultibotLayer::MultibotLayer() {}
11 | 
12 | 	void MultibotLayer::onInitialize()
13 | 	{
14 | 		ros::NodeHandle nh("~/" + name_);
15 | 		current_ = true;
16 | 
17 | 		dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
18 | 		dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
19 | 				&MultibotLayer::reconfigureCB, this, _1, _2);
20 | 		dsrv_->setCallback(cb);
21 | 	}
22 | 
23 | 
24 | 	void MultibotLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
25 | 	{
26 | 		enabled_ = config.enabled;
27 | 	}
28 | 
29 | 	void MultibotLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x,
30 | 			double* min_y, double* max_x, double* max_y)
31 | 	{
32 | 		if (!enabled_)
33 | 			return;
34 | 
35 | 
36 | 		nh.setParam("pose_x", origin_x);
37 | 		nh.setParam("pose_y", origin_y);
38 | 
39 | 		// dummy search for five robots. TODO: set dynamic robot array
40 | 		for (int i = 0; i < 5; i++) {
41 | 			std::string robot_pose = "/nb2_";
42 | 			std::string robot_x;
43 | 			std::string robot_y;
44 | 			std::string this_bot;
45 | 			// search for the full posename of the current robot. Eg, this_bot = "nb2_<current_robot_number>/pose_x".
46 | 			nh.searchParam("pose_x", this_bot);  
47 | 
48 | 
49 | 			robot_pose.append(std::to_string(i));  // set robot_pose to "/nb2_<i=1~5>"
50 | 			robot_x = robot_y = robot_pose;  // give robot_x and robot_y the same prefix
51 | 			robot_x.append("/pose_x");  // set robot_x to "/nb2_<i>/pose_x"
52 | 			robot_y.append("/pose_y");  // set robot_y to "/nb2_<i>/pose_y"
53 | 
54 | 			// Save poses of every robot, except itself, in mark[][] array
55 | 			if (nh.searchParam(robot_x, robot_x) && robot_x != this_bot) {
56 | 				nh.getParam(robot_x, mark[i][0]);
57 | 				nh.getParam(robot_y, mark[i][1]);
58 | 			} else {
59 | 				mark[i][0] = 100;  // Set an unaffected pose for unused robots
60 | 				mark[i][1] = 100;  // Set an unaffected pose for unused robots
61 | 			} 
62 | 
63 | 			*min_x = std::min(*min_x, mark[i][0]);
64 | 			*min_y = std::min(*min_y, mark[i][1]);
65 | 			*max_x = std::max(*max_x, mark[i][0]);
66 | 			*max_y = std::max(*max_y, mark[i][1]);
67 | 		}
68 | 	}
69 | 
70 | 	void MultibotLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
71 | 			int max_j)
72 | 	{
73 | 		if (!enabled_)
74 | 			return;
75 | 		unsigned int mx;
76 | 		unsigned int my;
77 | 		for (int i = 0; i < 5; i++) {
78 | 			if(master_grid.worldToMap(mark[i][0], mark[i][1], mx, my)){
79 | 				master_grid.setCost(mx, my, LETHAL_OBSTACLE);
80 | 			}
81 | 		}
82 | 	} 
83 | 
84 | } // end namespace
85 | 


--------------------------------------------------------------------------------