100 lines
3.0 KiB
C
100 lines
3.0 KiB
C
#ifndef COSTMAP_2D_TESTING_HELPER_H
|
|
#define COSTMAP_2D_TESTING_HELPER_H
|
|
|
|
#include<costmap_2d/cost_values.h>
|
|
#include<costmap_2d/costmap_2d.h>
|
|
#include <costmap_2d/static_layer.h>
|
|
#include <costmap_2d/obstacle_layer.h>
|
|
#include <costmap_2d/inflation_layer.h>
|
|
|
|
#include <sensor_msgs/point_cloud2_iterator.h>
|
|
|
|
const double MAX_Z(1.0);
|
|
|
|
char printableCost(unsigned char cost)
|
|
{
|
|
switch (cost)
|
|
{
|
|
case costmap_2d::NO_INFORMATION: return '?';
|
|
case costmap_2d::LETHAL_OBSTACLE: return 'L';
|
|
case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
|
|
case costmap_2d::FREE_SPACE: return '.';
|
|
default: return '0' + (unsigned char) (10 * cost / 255);
|
|
}
|
|
}
|
|
|
|
void printMap(costmap_2d::Costmap2D& costmap)
|
|
{
|
|
printf("map:\n");
|
|
for (int i = 0; i < costmap.getSizeInCellsY(); i++){
|
|
for (int j = 0; j < costmap.getSizeInCellsX(); j++){
|
|
printf("%4d", int(costmap.getCost(j, i)));
|
|
}
|
|
printf("\n\n");
|
|
}
|
|
}
|
|
|
|
unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
|
|
{
|
|
unsigned int count = 0;
|
|
for (int i = 0; i < costmap.getSizeInCellsY(); i++){
|
|
for (int j = 0; j < costmap.getSizeInCellsX(); j++){
|
|
unsigned char c = costmap.getCost(j, i);
|
|
if ((equal && c == value) || (!equal && c != value))
|
|
{
|
|
count+=1;
|
|
}
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
|
{
|
|
costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
|
|
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
|
|
slayer->initialize(&layers, "static", &tf);
|
|
}
|
|
|
|
costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
|
{
|
|
costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
|
|
olayer->initialize(&layers, "obstacles", &tf);
|
|
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
|
|
return olayer;
|
|
}
|
|
|
|
void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
|
|
double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
|
|
sensor_msgs::PointCloud2 cloud;
|
|
sensor_msgs::PointCloud2Modifier modifier(cloud);
|
|
modifier.setPointCloud2FieldsByString(1, "xyz");
|
|
modifier.resize(1);
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
|
|
*iter_x = x;
|
|
*iter_y = y;
|
|
*iter_z = z;
|
|
|
|
geometry_msgs::Point p;
|
|
p.x = ox;
|
|
p.y = oy;
|
|
p.z = oz;
|
|
|
|
costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0
|
|
olayer->addStaticObservation(obs, true, true);
|
|
}
|
|
|
|
costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
|
{
|
|
costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
|
|
ilayer->initialize(&layers, "inflation", &tf);
|
|
boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
|
|
layers.addPlugin(ipointer);
|
|
return ilayer;
|
|
}
|
|
|
|
|
|
#endif // COSTMAP_2D_TESTING_HELPER_H
|