Trouble with subscribing to a topic from a custom costmap layer plugin

edit

Hello,

I have created a new costmap layer by following the tutorial: http://wiki.ros.org/costmap_2d/Tutori..., where I have added a subscriber and callback function.

The purpose of this layer is to subscribe to a topic containing geometry_msg/Points, and add them to the costmap.

The following code shows simple_layers.cpp:

#include <simple_layer.h>
#include <pluginlib/class_list_macros.h>
#include <geometry_msgs/Point.h>
#include<vector>


using namespace std;

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

using costmap_2d::LETHAL_OBSTACLE;

namespace simple_layer_namespace
{

SimpleLayer::SimpleLayer() {}

vector<geometry_msgs::Point> costmapPoints;

//MY CALLBACK FUNCTION!
void SimpleLayer::formationCallback(const spot_pkg::formationPoints::ConstPtr& msg){
  ROS_INFO("Receiving points from callback");
  int size = msg->points.size();
  //Loop all points
  for(int i = 0; i < size; i++){

    geometry_msgs::Point newPoint = geometry_msgs::Point();
    newPoint.x = msg->points[i].x;
    newPoint.y = msg->points[i].y;

    costmapPoints.push_back(newPoint);
  }
}

void SimpleLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;
  //SUBSCRIBE TO
  sub = nh.subscribe("/topic", 1000, &SimpleLayer::formationCallback, this);


  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;

  ROS_INFO("Updating bounds");

  mark_x_ = robot_x + 2 *cos(robot_yaw);
  mark_y_ = robot_y + 2* 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;

  ROS_INFO("Updating costs");

  unsigned int mx;
  unsigned int my;


  for(int i = 0; i < costmapPoints.size(); i++){
    if(master_grid.worldToMap(costmapPoints[i].x, costmapPoints[i].y, mx, my)){
      master_grid.setCost(mx, my, LETHAL_OBSTACLE);
    }
  }
}


} // end namespace

The following shows costmap_common_params.yaml that starts my plugin.

obstacle_range: 10 #2.5
raytrace_range: 10.5 #3

footprint: [[-0.4, -0.2], [-0.4, 0.2], [0.4, 0.2], [0.4, -0.2]]

transform_tolerance: 1.0

inflation:
  inflation_radius: 0.075 #0.1
  cost_scaling_factor: 15  # exponential rate at which the obstacle cost drops off (default 10)

obstacle_2d_layer:
    observation_sources: scan
    scan: {data_type: LaserScan, sensor_frame: sensor_frame, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0, max_obstacle_height: 2.0}

plugins: 
    - {name: static_map, type: "costmap_2d::StaticLayer"}
    - {name: obstacle_2d_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation, type: "costmap_2d::InflationLayer"}
    - {name: simple_layer, type: "simple_layer_namespace::SimpleLayer"}

The layer loads correctly when launching move base, but the callback function is not called when information is published to the topic.