38 #include <sensor_msgs/PointCloud2.h> 45 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) {
55 sensor_msgs::PointCloud2 cost_cloud;
61 "y", 1, sensor_msgs::PointField::FLOAT32,
62 "z", 1, sensor_msgs::PointField::FLOAT32,
63 "path_cost", 1, sensor_msgs::PointField::FLOAT32,
64 "goal_cost", 1, sensor_msgs::PointField::FLOAT32,
65 "occ_cost", 1, sensor_msgs::PointField::FLOAT32,
66 "total_cost", 1, sensor_msgs::PointField::FLOAT32);
71 double x_coord, y_coord;
73 cloud_mod.
resize(x_size * y_size);
76 float path_cost, goal_cost, occ_cost, total_cost;
77 for (
unsigned int cx = 0; cx < x_size; cx++) {
78 for (
unsigned int cy = 0; cy < y_size; cy++) {
79 costmap_p_->
mapToWorld(cx, cy, x_coord, y_coord);
80 if (
cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
84 iter_x[3] = path_cost;
85 iter_x[4] = goal_cost;
87 iter_x[6] = total_cost;
MapGridVisualizer()
Default constructor.
unsigned int getSizeInCellsX() const
std::string name_
The name to get parameters relative to.
unsigned int getSizeInCellsY() 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...
void setPointCloud2Fields(int n_fields,...)
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 publish(const boost::shared_ptr< M > &message) const
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.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const