i am new to ROS and coding, i am trying to write a costmap layer which subscribes to topic "navmsgs/path"(from bag file which assumed as path of other robot) and mark those array of poses as obstacles and update to master layer so that robot avoids that path and replans the path. i have followed this tutorial http://wiki.ros.org/costmap2d/Tutorials/Creating%20a%20New%20Layer as the example, here i facing the problem in updating the correct boundary values in update bound function and mark those values in master layer. the obstacles are marked at some distance before the robot and when the robot is moving, (its showing in RViz) obstacle path is also moving along with the robot even though i mention the boundary values using markx and marky in functionCB and updating boundary values accordingly by calling set bounds function in it. please dont mind i dont know how to post the questions in answers.ros.org, please kindly help me in solving problem. thanks and regards

1. List item

my .cpp file

#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::setBounds(double x, double y, double z)
{
  *temp_min_x = std::min(*temp_min_x, x);
  *temp_min_y = std::min(*temp_min_y, y);
  *temp_max_x = std::max(*temp_max_x, x);
  *temp_max_y = std::max(*temp_max_y, y);
updateBounds(x,y,z,temp_min_x,temp_min_y, temp_max_x, temp_max_y);
}
void GridLayer::functionCB(const nav_msgs::Path& msg)
{
geometry_msgs::PoseStamped pose;
std::vector<geometry_msgs::PoseStamped> vector=msg.poses;
for(auto it=vector.begin(); it!=vector.end(); ++it)
  {
  double mark_x, mark_y, yaw;
  pose.pose=it->pose;
  mark_x=pose.pose.position.x;
  mark_y=pose.pose.position.y;
  yaw = pose.pose.orientation.z;
  my_bool = false;
  setBounds(mark_x,mark_y, yaw);
  unsigned int mx;
  unsigned int my;
    if(worldToMap(mark_x, mark_y, mx, my))
    {
     setCost(mx, my, LETHAL_OBSTACLE);
    }
  }
}
void GridLayer::onInitialize()
{
 ros::NodeHandle nh("~/" + name_);
  current_ = true;
  my_bool = true;
  default_value_ = NO_INFORMATION;
  matchSize();
  sub=nh.subscribe("/local_path", 100, &GridLayer::functionCB, this);
  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;
  if(my_bool)
  {
temp_min_x = min_x;
temp_max_y = min_y;
temp_max_x = max_x;
temp_max_y = max_y;
  }
  else
  {
    *min_x = std::min(*min_x, *temp_min_x);
    *min_y = std::min(*min_y, *temp_min_y);
    *max_x = std::max(*max_x, *temp_max_x);
    *max_y = std::max(*max_y, *temp_max_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

2 . List item

my .h file

#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>
#include <nav_msgs/Path.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;
  }
void functionCB(const nav_msgs::Path& msg);

  virtual void matchSize();
void setBounds(double x, double y, double z);
private:
  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
double mark_x, mark_y;
bool my_bool;
double *temp_min_x, *temp_min_y, *temp_max_x, *temp_max_y;

ros::Subscriber sub;
};
}
#endif