layered costmap : how to subscribe to nav_msgs/path, mark as obstacles and update to master layer

edit

i am new to ROS and coding, i am trying to write a costmap layer which subscribes to topic "nav_msgs/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/costmap_2d/Tutori... 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 mark_x and mark_y 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 ...