 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Creating a New Layer
Description: How to create a new layer in Hydro+ navigationKeywords: navigation
Tutorial Level: INTERMEDIATE
Contents
Simple Layer
Here we'll create a simple layer that just puts a lethal point on the costmap one meter in front of the robot.
Create the Package
catkin_create_pkg simple_layers roscpp costmap_2d dynamic_reconfigure
roscreate-pkg simple_layers roscpp costmap_2d dynamic_reconfigure
Header File
1 #ifndef SIMPLE_LAYER_H_
2 #define SIMPLE_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 simple_layer_namespace
10 {
11
12 class SimpleLayer : public costmap_2d::Layer
13 {
14 public:
15 SimpleLayer();
16
17 virtual void onInitialize();
18 virtual void updateBounds(double robot_x, double robot_y, double robot_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 double mark_x_, mark_y_;
26 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
27 };
28 }
29 #endif
30
Main Source Code
1 #include<simple_layers/simple_layer.h>
2 #include <pluginlib/class_list_macros.h>
3
4 PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)
5
6 using costmap_2d::LETHAL_OBSTACLE;
7
8 namespace simple_layer_namespace
9 {
10
11 SimpleLayer::SimpleLayer() {}
12
13 void SimpleLayer::onInitialize()
14 {
15 ros::NodeHandle nh("~/" + name_);
16 current_ = true;
17
18 dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
19 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
20 &SimpleLayer::reconfigureCB, this, _1, _2);
21 dsrv_->setCallback(cb);
22 }
23
24
25 void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
26 {
27 enabled_ = config.enabled;
28 }
29
30 void SimpleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
31 double* min_y, double* max_x, double* max_y)
32 {
33 if (!enabled_)
34 return;
35
36 mark_x_ = robot_x + cos(robot_yaw);
37 mark_y_ = robot_y + sin(robot_yaw);
38
39 *min_x = std::min(*min_x, mark_x_);
40 *min_y = std::min(*min_y, mark_y_);
41 *max_x = std::max(*max_x, mark_x_);
42 *max_y = std::max(*max_y, mark_y_);
43 }
44
45 void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
46 int max_j)
47 {
48 if (!enabled_)
49 return;
50 unsigned int mx;
51 unsigned int my;
52 if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
53 master_grid.setCost(mx, my, LETHAL_OBSTACLE);
54 }
55 }
56
57 } // end namespace
58
Initial Explanation
Use of Dynamic Reconfigure
This layer uses the GenericPluginConfig which consists of only a flag called enabled, for easy enabling and disabling of this particular layer. You can create your own custom dynamic_reconfigure configuration and insert it instead.
updateBounds
The updateBounds method does not change the costmap just yet. It just defines the area that will need to be updated. We calculate the point we want to change (mark_x_, mark_y_) and then expand the min/max bounds to be sure it includes the new point.
updateCosts
First, we calculate which grid cell our point is in using worldToMap. Then we set the cost of that cell. Pretty simple.
Compiling the Layer
Add the following to your CMakeLists.txt
rosbuild_add_library(simple_layer src/simple_layer.cpp)
include_directories(${catkin_INCLUDE_DIRS} include)
add_library(simple_layer src/simple_layer.cpp)
Plugin Declaration
Create an XML file, which here we're calling costmap_plugins.xml
1 <library path="lib/libsimple_layer">
2 <class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
3 <description>Demo Layer that adds a point 1 meter in front of the robot</description>
4 </class>
5 </library>
Changes to Metadata
These lines need to be added to your manifest.xml to get the list of plugins to be registered.
These lines need to be added to your package.xml to get the list of plugins to be registered.
Grid Layer
The previous 'layer' did not actually store any information other than the single point needed. If you want to store a full costmap in the layer, the approach may look something like this.
New Include
1 #ifndef GRID_LAYER_H_
2 #define GRID_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 simple_layer_namespace
10 {
11
12 class GridLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D
13 {
14 public:
15 GridLayer();
16
17 virtual void onInitialize();
18 virtual void updateBounds(double robot_x, double robot_y, double robot_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 bool isDiscretized()
22 {
23 return true;
24 }
25
26 virtual void matchSize();
27
28 private:
29 void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
30 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
31 };
32 }
33 #endif
34
Two key differences:
- This layer now extends the Costmap2D class
- isDiscretized is set to True (used for resizing)
New Source Code
1 #include<simple_layers/grid_layer.h>
2 #include <pluginlib/class_list_macros.h>
3
4 PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)
5
6 using costmap_2d::LETHAL_OBSTACLE;
7 using costmap_2d::NO_INFORMATION;
8
9 namespace simple_layer_namespace
10 {
11
12 GridLayer::GridLayer() {}
13
14 void GridLayer::onInitialize()
15 {
16 ros::NodeHandle nh("~/" + name_);
17 current_ = true;
18 default_value_ = NO_INFORMATION;
19 matchSize();
20
21 dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
22 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
23 &GridLayer::reconfigureCB, this, _1, _2);
24 dsrv_->setCallback(cb);
25 }
26
27
28 void GridLayer::matchSize()
29 {
30 Costmap2D* master = layered_costmap_->getCostmap();
31 resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
32 master->getOriginX(), master->getOriginY());
33 }
34
35
36 void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
37 {
38 enabled_ = config.enabled;
39 }
40
41 void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
42 double* min_y, double* max_x, double* max_y)
43 {
44 if (!enabled_)
45 return;
46
47 double mark_x = robot_x + cos(robot_yaw), mark_y = robot_y + sin(robot_yaw);
48 unsigned int mx;
49 unsigned int my;
50 if(worldToMap(mark_x, mark_y, mx, my)){
51 setCost(mx, my, LETHAL_OBSTACLE);
52 }
53
54 *min_x = std::min(*min_x, mark_x);
55 *min_y = std::min(*min_y, mark_y);
56 *max_x = std::max(*max_x, mark_x);
57 *max_y = std::max(*max_y, mark_y);
58 }
59
60 void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
61 int max_j)
62 {
63 if (!enabled_)
64 return;
65
66 for (int j = min_j; j < max_j; j++)
67 {
68 for (int i = min_i; i < max_i; i++)
69 {
70 int index = getIndex(i, j);
71 if (costmap_[index] == NO_INFORMATION)
72 continue;
73 master_grid.setCost(i, j, costmap_[index]);
74 }
75 }
76 }
77
78 } // end namespace
79
Fairly similar, but note the differences:
- In initialization, we set default_value_ and call matchSize. Match size matches the size of the master grid and fills it with the value specified in default_value_, which by default is zero.
- We also implement the matchSize method to do what we said above. This is left to the user to implement for cases where additional adjustments need to be made.
- Instead of setting the value in updateCosts, we set the value in the layer's own costmap using the setCost method.
- Now in the updateCosts method, we overwrite the previous values of the master costmap to include our marks. We do not overwrite if the layer's costmap value is NO_INFORMATION.
Additional Changes
You must build the new layer similarly in CMakelists.txt.
rosbuild_add_library(grid_layer src/grid_layer.cpp)
add_library(grid_layer src/grid_layer.cpp)
Also, costmap_plugins.xml is updated to now include both libraries.
1 <class_libraries>
2 <library path="lib/libsimple_layer">
3 <class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
4 <description>Demo Layer that adds a point 1 meter in front of the robot</description>
5 </class>
6 </library>
7 <library path="lib/libgrid_layer">
8 <class type="simple_layer_namespace::GridLayer" base_class_type="costmap_2d::Layer">
9 <description>Demo Layer that marks all points that were ever one meter in front of the robot</description>
10 </class>
11 </library>
12 </class_libraries>
