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

asked 2018-12-17 22:02:25 -0500

saito gravatar image

updated 2018-12-18 03:11:53 -0500

mgruhler gravatar image

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 cordfigure

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason duplicate question by saito
close date 2019-01-07 22:22:12.831711

Comments

just to avoid an xy-problem: What is your actual goal? What do you want to achieve with doing the above? Please update your question with some details on this.

mgruhler gravatar imagemgruhler ( 2018-12-18 01:23:16 -0500 )edit

Thanks for the reply. That is exactly as you point out. I updated my question. Please answer.

saito gravatar imagesaito ( 2018-12-18 02:09:45 -0500 )edit