####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Creating a New Layer
## multi-line description to be displayed in search 
## description = How to create a new layer in Hydro+ navigation
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords = navigation
####################################
<<Buildsystem()>>
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== 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 ===
{{{{#!wiki buildsystem catkin
catkin_create_pkg simple_layers roscpp costmap_2d dynamic_reconfigure
}}}}
{{{{#!wiki buildsystem rosbuild
roscreate-pkg simple_layers roscpp costmap_2d dynamic_reconfigure
}}}}
=== Header File ===

{{{#!cplusplus
#ifndef SIMPLE_LAYER_H_
#define SIMPLE_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>

namespace simple_layer_namespace
{

class SimpleLayer : public costmap_2d::Layer
{
public:
  SimpleLayer();

  virtual void onInitialize();
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
                             double* max_y);
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

private:
  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);

  double mark_x_, mark_y_;
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif

}}}

=== Main Source Code ===
{{{#!cplusplus
#include<simple_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;

namespace simple_layer_namespace
{

SimpleLayer::SimpleLayer() {}

void SimpleLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &SimpleLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void SimpleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  if (!enabled_)
    return;

  mark_x_ = robot_x + cos(robot_yaw);
  mark_y_ = robot_y + sin(robot_yaw);

  *min_x = std::min(*min_x, mark_x_);
  *min_y = std::min(*min_y, mark_y_);
  *max_x = std::max(*max_x, mark_x_);
  *max_y = std::max(*max_y, mark_y_);
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}

} // end namespace
}}}

=== 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

{{{{#!wiki buildsystem rosbuild
{{{
rosbuild_add_library(simple_layer src/simple_layer.cpp)
}}}
}}}}

{{{{#!wiki buildsystem catkin
{{{
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

{{{#!xml
<library path="lib/libsimple_layer">
  <class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
    <description>Demo Layer that adds a point 1 meter in front of the robot</description>
  </class>
</library>

}}}

=== Changes to Metadata ===

{{{{#!wiki buildsystem rosbuild
These lines need to be added to your manifest.xml to get the list of plugins to be registered. 
}}}}
{{{{#!wiki buildsystem catkin
These lines need to be added to your package.xml to get the list of plugins to be registered. 
}}}}

{{{#!xml
  <export>
    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
  </export>
}}}

== 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 ===
{{{#!cplusplus
#ifndef GRID_LAYER_H_
#define GRID_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>

namespace simple_layer_namespace
{

class GridLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D
{
public:
  GridLayer();

  virtual void onInitialize();
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
                             double* max_y);
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
  bool isDiscretized()
  {
    return true;
  }

  virtual void matchSize();

private:
  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif

}}}

Two key differences:
 * This layer now extends the Costmap2D class
 * isDiscretized is set to True (used for resizing)

=== New Source Code ===
{{{#!cplusplus
#include<simple_layers/grid_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;

namespace simple_layer_namespace
{

GridLayer::GridLayer() {}

void GridLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;
  default_value_ = NO_INFORMATION;
  matchSize();

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &GridLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void GridLayer::matchSize()
{
  Costmap2D* master = layered_costmap_->getCostmap();
  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
            master->getOriginX(), master->getOriginY());
}


void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  if (!enabled_)
    return;

  double mark_x = robot_x + cos(robot_yaw), mark_y = robot_y + sin(robot_yaw);
  unsigned int mx;
  unsigned int my;
  if(worldToMap(mark_x, mark_y, mx, my)){
    setCost(mx, my, LETHAL_OBSTACLE);
  }
  
  *min_x = std::min(*min_x, mark_x);
  *min_y = std::min(*min_y, mark_y);
  *max_x = std::max(*max_x, mark_x);
  *max_y = std::max(*max_y, mark_y);
}

void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;

  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
    {
      int index = getIndex(i, j);
      if (costmap_[index] == NO_INFORMATION)
        continue;
      master_grid.setCost(i, j, costmap_[index]); 
    }
  }
}

} // end namespace
}}}
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. 

{{{{#!wiki buildsystem rosbuild
{{{
rosbuild_add_library(grid_layer src/grid_layer.cpp)
}}}
}}}}

{{{{#!wiki buildsystem catkin
{{{
add_library(grid_layer src/grid_layer.cpp)
}}}
}}}}

Also, costmap_plugins.xml is updated to now include both libraries. 
{{{#!xml
<class_libraries>
  <library path="lib/libsimple_layer">
    <class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
      <description>Demo Layer that adds a point 1 meter in front of the robot</description>
    </class>
  </library>
  <library path="lib/libgrid_layer">
    <class type="simple_layer_namespace::GridLayer" base_class_type="costmap_2d::Layer">
      <description>Demo Layer that marks all points that were ever one meter in front of the robot</description>
    </class>
  </library>
</class_libraries>
}}}

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE