44 void MapGridVisualizer::initialize(
const std::string& name, std::string frame_id, boost::function<
bool (
int cx,
int cy,
float &path_cost,
float &goal_cost,
float &occ_cost,
float &total_cost)> cost_function) {
50 cost_cloud_ =
new pcl::PointCloud<MapGridCostPoint>;
59 double x_coord, y_coord;
65 float path_cost, goal_cost, occ_cost, total_cost;
66 for (
unsigned int cx = 0; cx < x_size; cx++) {
67 for (
unsigned int cy = 0; cy < y_size; cy++) {
68 costmap_p_->
mapToWorld(cx, cy, x_coord, y_coord);
69 if (
cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
MapGridVisualizer()
Default constructor.
pcl::PointCloud< MapGridCostPoint > * cost_cloud_
std::string name_
The name to get parameters relative to.
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
void publishCostCloud(const costmap_2d::Costmap2D *costmap_p_)
Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points fo...
pcl_ros::Publisher< MapGridCostPoint > pub_
void initialize(const std::string &name, std::string frame, boost::function< bool(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function)
Initializes the MapGridVisualizer.
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
boost::function< bool(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function_
The function to be used to generate the cost components for the output PointCloud.
unsigned int getSizeInCellsY() const
unsigned int getSizeInCellsX() const
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)