1 #ifndef COSTMAP_2D_TESTING_HELPER_H 2 #define COSTMAP_2D_TESTING_HELPER_H 10 const double MAX_Z(1.0);
17 costmap.
setCost(j, i, map[index]);
30 default:
return '0' + (
unsigned char) (10 * cost / 255);
39 printf(
"%4d",
int(costmap.
getCost(j, i)));
47 unsigned int count = 0;
50 unsigned char c = costmap.
getCost(j, i);
51 if ((equal && c == value) || (!equal && c != value))
76 double ox = 0.0,
double oy = 0.0,
double oz =
MAX_Z){
77 pcl::PointCloud<pcl::PointXYZ> cloud;
78 cloud.points.resize(1);
79 cloud.points[0].x = x;
80 cloud.points[0].y = y;
81 cloud.points[0].z =
z;
83 geometry_msgs::Point p;
102 #endif // COSTMAP_2D_TESTING_HELPER_H void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal=true)
void initialize(LayeredCostmap *parent, std::string name, tf::TransformListener *tf)
char printableCost(unsigned char cost)
costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
Stores an observation in terms of a point cloud and the origin of the source.
void addStaticLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char FREE_SPACE
void addStaticObservation(costmap_2d::Observation &obs, bool marking, bool clearing)
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
void addPlugin(boost::shared_ptr< Layer > plugin)
void setValues(costmap_2d::Costmap2D &costmap, const unsigned char *map)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
TFSIMD_FORCE_INLINE const tfScalar & z() const
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
static const unsigned char LETHAL_OBSTACLE
costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char NO_INFORMATION
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)
A 2D costmap provides a mapping between points in the world and their associated "costs".
void printMap(costmap_2d::Costmap2D &costmap)
Instantiates different layer plugins and aggregates them into one score.