I would like to create a plugin that remodeled the inflation layer of costmap _ 2 d. [closed]

edit

Hello. I am a ROS beginner. I am doing SLAM of turtlebot 2 in the ubuntu 14.04, ros (indigo) personal computer environment.

I am trying to implement the cost I thought using my plugin in the costmap _ 2 d package. I would like to create a plug-in that increases the cost map cost by blind spots such as corners. A schematic is shown in the figure. I wrote a program for center of dangerous(x,y). Its cost expands concentrically from center of dangerous.

So we want to propagate costs from center of dangerous (x, y) based on the inflation layer. However, I do not know where in this layer to improve. Thank you for those who are familiar with the inflation layer.

inflation layer source cord

I will attach a source code asking for center of dangerous for reference.

#include <algorithm>
#include<occlusion_layers/occlusion_layer.h>
#include <pluginlib/class_list_macros.h>
#include <costmap_2d/costmap_math.h>
#include <sensor_msgs/LaserScan.h>

PLUGINLIB_EXPORT_CLASS(occlusion_layer_namespace::OcclusionLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  size_t i=0;
  double angle_min = msg->angle_min;
  double angle_max = msg->angle_max;
  double angle_increment = msg->angle_increment;

  double range_min = msg->range_min;
  double range_max = msg->range_max;
  double r[1000];
  double x[1000];
  double y[1000];
  double theta[1000];

  size_t n=0;
  double new_r[1000];
  double new_x[1000];
  double new_y[1000];
  double new_theta[1000];
  size_t end_p=0;

  size_t ro=0;
  size_t lo=0;
  size_t center_p=0;
  size_t occ_lp[100];
  size_t occ_rp[100];

//
  FILE *fp;
  fp =fopen("URG_Data.dat","w");

 //std::cout << "range_max: " << range_max << std::endl;　

  size_t data_length = (angle_max - angle_min) / angle_increment;
  for ( i = 0; i < data_length; i++) {

     //
    r[i] = std::min(range_max, std::max(range_min, (double)msg->ranges[i]));
    x[i] = r[i] * std::cos(angle_min + i * angle_increment);
    y[i] = r[i] * std::sin(angle_min + i * angle_increment);
    theta[i] = angle_min + i * angle_increment;

//
  std::cout <<"x"<<i <<":"<< x[i] << std::endl;
  std::cout <<"y"<<i <<":"<< y[i] << std::endl;


//error 0.2m 
   if(r[i]>=0.2){
    new_r[n]=r[i];
    new_x[n]=x[i]; 
    new_y[n]=y[i];
    new_theta[n]=theta[i];
fprintf(fp,"%zu %lf %lf %lf %lf \n",n,new_r[n],new_x[n],new_y[n],new_theta[n]);
    n=n+1;
     }
  }//

//center
for(i=1; i<n; i++){
center_p = i;
if(theta[i]>0){break;}
}

//right occlusion
for(i=0; i<center_p-1; i++){
if((new_r[i+1]-new_r[i])>1||(new_r[i]-new_r[i+1])>1){
 occ_rp[ro]=i;
fprintf(fp,"%zu \n",occ_rp[ro]);
 ro=ro+1;
}}

//left occlusion
for(i=center_p; i<n-1; i++){
if((new_r[i+1]-new_r[i])>1||(new_r[i]-new_r[i+1])>1){
 occ_lp[lo]=i+1;
 fprintf(fp,"%zu \n",occ_lp[lo]);
 lo=lo+1;
}}

//right danger point
double dan_rx[100];
double dan_ry[100];
for(i=0; i<ro; i++){
  dan_rx[i]=new_x[occ_rp[i]];
  dan_ry[i]=new_y[occ_rp[i]]+0.4* std::tan(new_theta[occ_rp[i]]);
  fprintf(fp,"%lf %lf \n",dan_rx[i],dan_ry[i]);
}

//left danger point
double dan_lx[100];
double dan_ly[100];
for(i=0; i<lo; i++){
  dan_lx ...