Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <base_local_planner/map_grid_visualizer.h>
00035 #include <base_local_planner/map_cell.h>
00036 #include <vector>
00037
00038 #include <pcl_conversions/pcl_conversions.h>
00039
00040 namespace base_local_planner {
00041 MapGridVisualizer::MapGridVisualizer() {}
00042
00043
00044 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) {
00045 name_ = name;
00046 cost_function_ = cost_function;
00047
00048 ns_nh_ = ros::NodeHandle("~/" + name_);
00049
00050 cost_cloud_ = new pcl::PointCloud<MapGridCostPoint>;
00051 cost_cloud_->header.frame_id = frame_id;
00052 pub_.advertise(ns_nh_, "cost_cloud", 1);
00053 }
00054
00055 void MapGridVisualizer::publishCostCloud(const costmap_2d::Costmap2D* costmap_p_) {
00056 unsigned int x_size = costmap_p_->getSizeInCellsX();
00057 unsigned int y_size = costmap_p_->getSizeInCellsY();
00058 double z_coord = 0.0;
00059 double x_coord, y_coord;
00060 MapGridCostPoint pt;
00061 cost_cloud_->points.clear();
00062 std_msgs::Header header = pcl_conversions::fromPCL(cost_cloud_->header);
00063 header.stamp = ros::Time::now();
00064 cost_cloud_->header = pcl_conversions::toPCL(header);
00065 float path_cost, goal_cost, occ_cost, total_cost;
00066 for (unsigned int cx = 0; cx < x_size; cx++) {
00067 for (unsigned int cy = 0; cy < y_size; cy++) {
00068 costmap_p_->mapToWorld(cx, cy, x_coord, y_coord);
00069 if (cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
00070 pt.x = x_coord;
00071 pt.y = y_coord;
00072 pt.z = z_coord;
00073 pt.path_cost = path_cost;
00074 pt.goal_cost = goal_cost;
00075 pt.occ_cost = occ_cost;
00076 pt.total_cost = total_cost;
00077 cost_cloud_->push_back(pt);
00078 }
00079 }
00080 }
00081 pub_.publish(*cost_cloud_);
00082 ROS_DEBUG("Cost PointCloud published");
00083 }
00084 };