34 #ifndef MAP_GRID_VISUALIZER_H_ 35 #define MAP_GRID_VISUALIZER_H_ 57 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);
71 boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)>
cost_function_;
MapGridVisualizer()
Default constructor.
pcl::PointCloud< MapGridCostPoint > * cost_cloud_
std::string name_
The name to get parameters relative to.
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.
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.