DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:24:18 +07:00
parent 4246453ae6
commit 72b2f3c639
49 changed files with 476 additions and 476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
include/costmap_2d/critical_layer.h
View File

@@ -1,19 +0,0 @@
#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
#define COSTMAP_2D_CRITICAL_LAYER_H_
#include <costmap_2d/static_layer.h>
namespace costmap_2d
{
class CriticalLayer : public StaticLayer
{
public:
CriticalLayer();
virtual ~CriticalLayer();
private:
unsigned char interpretValue(unsigned char value) override;
void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
};
}
#endif // COSTMAP_2D_CRITICAL_LAYER_H_

18
include/costmap_2d/preferred_layer.h
View File

@@ -1,18 +0,0 @@
#ifndef COSTMAP_2D_PREFERRED_LAYER_H_
#define COSTMAP_2D_PREFERRED_LAYER_H_
#include <costmap_2d/static_layer.h>
namespace costmap_2d
{
class PreferredLayer : public StaticLayer
{
public:
PreferredLayer();
virtual ~PreferredLayer();
private:
unsigned char interpretValue(unsigned char value);
};
}
#endif // COSTMAP_2D_PREFERRED_LAYER_

99
include/costmap_2d/testing_helper.h
View File

@@ -1,99 +0,0 @@
#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 <robot_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, tf3::BufferCore& 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, tf3::BufferCore& 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){
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(1);
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = x;
*iter_y = y;
*iter_z = z;
robot_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, tf3::BufferCore& 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

21
include/costmap_2d/unpreferred_layer.h
View File

@@ -1,21 +0,0 @@
#ifndef COSTMAP_2D_UNPREFERRED_LAYER_H_
#define COSTMAP_2D_UNPREFERRED_LAYER_H_
#include <costmap_2d/static_layer.h>
namespace costmap_2d
{
class UnPreferredLayer : public StaticLayer
{
public:
UnPreferredLayer();
virtual ~UnPreferredLayer();
private:
unsigned char interpretValue(unsigned char value);
};
}
#endif // COSTMAP_2D_UNPREFERRED_LAYER_

10
include/costmap_2d/array_parser.h → include/robot_costmap_2d/array_parser.h
View File

@@ -28,13 +28,13 @@
*
* author: Dave Hershberger
*/
#ifndef COSTMAP_2D_ARRAY_PARSER_H
#define COSTMAP_2D_ARRAY_PARSER_H
#ifndef ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#define ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#include <vector>
#include <string>
namespace costmap_2d
namespace robot_costmap_2d
{
/** @brief Parse a vector of vectors of floats from a string.
@@ -46,6 +46,6 @@ namespace costmap_2d
* anything, like part of a successful parse. */
std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
} // end namespace costmap_2d
} // end namespace robot_costmap_2d
#endif // COSTMAP_2D_ARRAY_PARSER_H
#endif // ROBOT_COSTMAP_2D_ARRAY_PARSER_H

8
include/costmap_2d/cost_values.h → include/robot_costmap_2d/cost_values.h
View File

@@ -34,10 +34,10 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef COSTMAP_2D_COST_VALUES_H_
#define COSTMAP_2D_COST_VALUES_H_
#ifndef ROBOT_COSTMAP_2D_COST_VALUES_H_
#define ROBOT_COSTMAP_2D_COST_VALUES_H_
/** Provides a mapping for often used cost values */
namespace costmap_2d
namespace robot_costmap_2d
{
static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
@@ -47,4 +47,4 @@ static const unsigned char FREE_SPACE = 60;
static const unsigned char PREFERRED_SPACE = 20;
static const unsigned char UNPREFERRED_SPACE = 100;
}
#endif // COSTMAP_2D_COST_VALUES_H_
#endif // ROBOT_COSTMAP_2D_COST_VALUES_H_

10
include/costmap_2d/costmap_2d.h → include/robot_costmap_2d/costmap_2d.h
View File

@@ -35,15 +35,15 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_H_
#define COSTMAP_2D_COSTMAP_2D_H_
#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#include <vector>
#include <queue>
#include <robot_geometry_msgs/Point.h>
#include <boost/thread.hpp>
namespace costmap_2d
namespace robot_costmap_2d
{
// convenient for storing x/y point pairs
@@ -464,6 +464,6 @@ protected:
std::vector<MapLocation>& cells_;
};
};
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_H
#endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H

20
include/costmap_2d/costmap_2d_robot.h → include/robot_costmap_2d/costmap_2d_robot.h
View File

@@ -35,12 +35,12 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
@@ -49,7 +49,7 @@
#include <tf3/LinearMath/Transform.h>
#include <robot/rate.h>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
@@ -70,7 +70,7 @@ public:
}
};
namespace costmap_2d
namespace robot_costmap_2d
{
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
@@ -182,7 +182,7 @@ public:
/** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */
robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
{
return costmap_2d::toPolygon(padded_footprint_);
return robot_costmap_2d::toPolygon(padded_footprint_);
}
/** @brief Return the current footprint of the robot as a vector of points.
@@ -275,6 +275,6 @@ private:
void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
};
// class Costmap2DROBOT
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_ROBOT_H
#endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H

22
include/costmap_2d/costmap_layer.h → include/robot_costmap_2d/costmap_layer.h
View File

@@ -35,13 +35,13 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#define ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class CostmapLayer : public Layer, public Costmap2D
@@ -79,7 +79,7 @@ protected:
* TrueOverwrite means every value from this layer
* is written into the master grid.
*/
void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
void updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
@@ -88,7 +88,7 @@ protected:
* Overwrite means every valid value from this layer
* is written into the master grid (does not copy NO_INFORMATION)
*/
void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
void updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
@@ -99,7 +99,7 @@ protected:
* it is overwritten. If the layer's value is NO_INFORMATION,
* the master value does not change.
*/
void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
void updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
@@ -113,7 +113,7 @@ protected:
* If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
* the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).
*/
void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
void updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/**
* Updates the bounding box specified in the parameters to include
@@ -147,5 +147,5 @@ private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_LAYER_H_
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_

6
include/costmap_2d/costmap_math.h → include/robot_costmap_2d/costmap_math.h
View File

@@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_MATH_H_
#define COSTMAP_2D_COSTMAP_MATH_H_
#ifndef ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#define ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#include <math.h>
#include <algorithm>
@@ -66,4 +66,4 @@ bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, f
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2);
#endif // COSTMAP_2D_COSTMAP_MATH_H_
#endif // ROBOT_COSTMAP_2D_COSTMAP_MATH_H_

19
include/robot_costmap_2d/critical_layer.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
#define ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
#include <robot_costmap_2d/static_layer.h>
namespace robot_costmap_2d
{
class CriticalLayer : public StaticLayer
{
public:
CriticalLayer();
virtual ~CriticalLayer();
private:
unsigned char interpretValue(unsigned char value) override;
void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
};
}
#endif // ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_

10
include/costmap_2d/directional_layer.h → include/robot_costmap_2d/directional_layer.h
View File

@@ -1,10 +1,10 @@
#ifndef COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#include <costmap_2d/static_layer.h>
#include <robot_costmap_2d/static_layer.h>
#include <robot_nav_msgs/Path.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class DirectionalLayer : public StaticLayer
{
@@ -32,4 +32,4 @@ namespace costmap_2d
};
}
#endif // COSTMAP_2D_DIRECTIONAL_LAYER_H_
#endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_

10
include/costmap_2d/footprint.h → include/robot_costmap_2d/footprint.h
View File

@@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H
#ifndef ROBOT_COSTMAP_2D_FOOTPRINT_H
#define ROBOT_COSTMAP_2D_FOOTPRINT_H
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
@@ -46,7 +46,7 @@
#include <robot/node_handle.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
@@ -144,6 +144,6 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
// * will see the new value. */
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint);
} // end namespace costmap_2d
} // end namespace robot_costmap_2d
#endif // COSTMAP_2D_FOOTPRINT_H
#endif // ROBOT_COSTMAP_2D_FOOTPRINT_H

16
include/costmap_2d/inflation_layer.h → include/robot_costmap_2d/inflation_layer.h
View File

@@ -35,14 +35,14 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <boost/thread.hpp>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
* @class CellData
@@ -84,7 +84,7 @@ public:
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual bool isDiscretized()
{
return true;
@@ -192,6 +192,6 @@ private:
bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
};
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_INFLATION_LAYER_H_
#endif // ROBOT_COSTMAP_2D_INFLATION_LAYER_H_

16
include/costmap_2d/layer.h → include/robot_costmap_2d/layer.h
View File

@@ -34,16 +34,16 @@
*
* Author: David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_LAYER_H_
#define COSTMAP_2D_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_LAYER_H_
#define ROBOT_COSTMAP_2D_LAYER_H_
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/utils.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/utils.h>
#include <string>
#include <tf3/buffer_core.h>
#include <robot/node_handle.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class LayeredCostmap;
@@ -164,6 +164,6 @@ private:
using PluginLayerPtr = boost::shared_ptr<Layer>;
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_LAYER_H_
#endif // ROBOT_COSTMAP_2D_LAYER_H_

18
include/costmap_2d/layered_costmap.h → include/robot_costmap_2d/layered_costmap.h
View File

@@ -35,16 +35,16 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_
#ifndef ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#define ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#include <costmap_2d/cost_values.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
namespace costmap_2d
namespace robot_costmap_2d
{
class Layer;
@@ -101,7 +101,7 @@ public:
bool isTrackingUnknown()
{
return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
return costmap_.getDefaultValue() == robot_costmap_2d::NO_INFORMATION;
}
std::vector<boost::shared_ptr<Layer> >* getPlugins()
@@ -172,6 +172,6 @@ private:
std::vector<robot_geometry_msgs::Point> footprint_;
};
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_LAYERED_COSTMAP_H_
#endif // ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_

10
include/costmap_2d/observation.h → include/robot_costmap_2d/observation.h
View File

@@ -29,13 +29,13 @@
* Authors: Conor McGann
*/
#ifndef COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_H_
#include <robot_geometry_msgs/Point.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
@@ -98,5 +98,5 @@ public:
double obstacle_range_, raytrace_range_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSERVATION_H_
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_OBSERVATION_H_

12
include/costmap_2d/observation_buffer.h → include/robot_costmap_2d/observation_buffer.h
View File

@@ -34,14 +34,14 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
#define COSTMAP_2D_OBSERVATION_BUFFER_H_
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector>
#include <list>
#include <string>
#include <robot/time.h>
#include <costmap_2d/observation.h>
#include <robot_costmap_2d/observation.h>
#include <tf3/buffer_core.h>
#include <robot_sensor_msgs/PointCloud2.h>
@@ -49,7 +49,7 @@
// Thread support
#include <boost/thread.hpp>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
* @class ObservationBuffer
@@ -150,5 +150,5 @@ private:
double obstacle_range_, raytrace_range_;
double tf_tolerance_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

40
include/costmap_2d/obstacle_layer.h → include/robot_costmap_2d/obstacle_layer.h
View File

@@ -35,13 +35,13 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_
#define COSTMAP_2D_OBSTACLE_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/costmap_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_nav_msgs/OccupancyGrid.h>
@@ -51,7 +51,7 @@
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class ObstacleLayer : public CostmapLayer
@@ -66,14 +66,14 @@ public:
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void activate();
virtual void deactivate();
virtual void reset();
// for testing purposes
void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
void clearStaticObservations(bool marking, bool clearing);
protected:
@@ -103,7 +103,7 @@ protected:
* @param buffer A pointer to the observation buffer to update
*/
void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering PointCloud2 messages
@@ -111,21 +111,21 @@ protected:
* @param buffer A pointer to the observation buffer to update
*/
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
/**
* @brief Get the observations used to mark space
* @param marking_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const;
bool getMarkingObservations(std::vector<robot_costmap_2d::Observation>& marking_observations) const;
/**
* @brief Get the observations used to clear space
* @param clearing_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
/**
* @brief Clear freespace based on one observation
@@ -135,7 +135,7 @@ protected:
* @param max_x
* @param max_y
*/
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
@@ -151,12 +151,12 @@ protected:
laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
// Used only for testing purposes
std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
bool rolling_window_;
bool stop_receiving_data_ = false;
@@ -167,6 +167,6 @@ private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
};
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_
#endif // ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_

18
include/robot_costmap_2d/preferred_layer.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
#define ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
#include <robot_costmap_2d/static_layer.h>
namespace robot_costmap_2d
{
class PreferredLayer : public StaticLayer
{
public:
PreferredLayer();
virtual ~PreferredLayer();
private:
unsigned char interpretValue(unsigned char value);
};
}
#endif // ROBOT_COSTMAP_2D_PREFERRED_LAYER_

16
include/costmap_2d/static_layer.h → include/robot_costmap_2d/static_layer.h
View File

@@ -35,17 +35,17 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_STATIC_LAYER_H_
#define COSTMAP_2D_STATIC_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#define ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/costmap_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <string>
namespace costmap_2d
namespace robot_costmap_2d
{
class StaticLayer : public CostmapLayer
@@ -60,7 +60,7 @@ public:
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void matchSize();
protected:
@@ -91,6 +91,6 @@ private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
};
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_STATIC_LAYER_H_
#endif // ROBOT_COSTMAP_2D_STATIC_LAYER_H_

99
include/robot_costmap_2d/testing_helper.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef ROBOT_COSTMAP_2D_TESTING_HELPER_H
#define ROBOT_COSTMAP_2D_TESTING_HELPER_H
#include<robot_costmap_2d/cost_values.h>
#include<robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
const double MAX_Z(1.0);
char printableCost(unsigned char cost)
{
switch (cost)
{
case robot_costmap_2d::NO_INFORMATION: return '?';
case robot_costmap_2d::LETHAL_OBSTACLE: return 'L';
case robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
case robot_costmap_2d::FREE_SPACE: return '.';
default: return '0' + (unsigned char) (10 * cost / 255);
}
}
void printMap(robot_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(robot_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(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
robot_costmap_2d::StaticLayer* slayer = new robot_costmap_2d::StaticLayer();
layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(slayer));
slayer->initialize(&layers, "static", &tf);
}
robot_costmap_2d::ObstacleLayer* addObstacleLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
robot_costmap_2d::ObstacleLayer* olayer = new robot_costmap_2d::ObstacleLayer();
olayer->initialize(&layers, "obstacles", &tf);
layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(olayer));
return olayer;
}
void addObservation(robot_costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(1);
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = x;
*iter_y = y;
*iter_z = z;
robot_geometry_msgs::Point p;
p.x = ox;
p.y = oy;
p.z = oz;
robot_costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0
olayer->addStaticObservation(obs, true, true);
}
robot_costmap_2d::InflationLayer* addInflationLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
robot_costmap_2d::InflationLayer* ilayer = new robot_costmap_2d::InflationLayer();
ilayer->initialize(&layers, "inflation", &tf);
boost::shared_ptr<robot_costmap_2d::Layer> ipointer(ilayer);
layers.addPlugin(ipointer);
return ilayer;
}
#endif // ROBOT_COSTMAP_2D_TESTING_HELPER_H

21
include/robot_costmap_2d/unpreferred_layer.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
#define ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
#include <robot_costmap_2d/static_layer.h>
namespace robot_costmap_2d
{
class UnPreferredLayer : public StaticLayer
{
public:
UnPreferredLayer();
virtual ~UnPreferredLayer();
private:
unsigned char interpretValue(unsigned char value);
};
}
#endif // ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_

8
include/costmap_2d/utils.h → include/robot_costmap_2d/utils.h
View File

@@ -1,11 +1,11 @@
#ifndef COSTMAP_2D_UTILS_H
#define COSTMAP_2D_UTILS_H
#ifndef ROBOT_COSTMAP_2D_UTILS_H
#define ROBOT_COSTMAP_2D_UTILS_H
#include <yaml-cpp/yaml.h>
#include <filesystem>
#include <string>
#include <iostream>
namespace costmap_2d
namespace robot_costmap_2d
{
template<typename T>
T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
@@ -64,4 +64,4 @@ namespace costmap_2d
}
#endif // COSTMAP_2D_UTILS_H
#endif // ROBOT_COSTMAP_2D_UTILS_H

8
include/costmap_2d/voxel_grid.h → include/robot_costmap_2d/voxel_grid.h
View File

@@ -6,15 +6,15 @@
// uint32 size_y
// uint32 size_z
#ifndef VOXEL_GRID_COSTMAP_2D_H
#define VOXEL_GRID_COSTMAP_2D_H
#ifndef VOXEL_GRID_ROBOT_COSTMAP_2D_H
#define VOXEL_GRID_ROBOT_COSTMAP_2D_H
#include <vector>
#include <robot_std_msgs/Header.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Vector3.h>
namespace costmap_2d
namespace robot_costmap_2d
{
struct VoxelGrid
{
@@ -29,4 +29,4 @@ namespace costmap_2d
}
#endif // VOXEL_GRID_COSTMAP_2D_H
#endif // VOXEL_GRID_ROBOT_COSTMAP_2D_H

22
include/costmap_2d/voxel_layer.h → include/robot_costmap_2d/voxel_layer.h
View File

@@ -35,23 +35,23 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_VOXEL_LAYER_H_
#define COSTMAP_2D_VOXEL_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_grid.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/voxel_grid.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
#include <costmap_2d/obstacle_layer.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class VoxelLayer : public ObstacleLayer
@@ -85,7 +85,7 @@ protected:
private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
@@ -137,6 +137,6 @@ private:
}
};
} // namespace costmap_2d
} // namespace robot_costmap_2d
#endif // COSTMAP_2D_VOXEL_LAYER_H_
#endif // ROBOT_COSTMAP_2D_VOXEL_LAYER_H_