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 namespace base_local_planner {
00039 MapGridVisualizer::MapGridVisualizer() {}
00040
00041
00042 void MapGridVisualizer::initialize(const std::string& name, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function) {
00043 name_ = name;
00044 cost_function_ = cost_function;
00045
00046 ns_nh_ = ros::NodeHandle("~/" + name_);
00047 ns_nh_.param("global_frame_id", frame_id_, std::string("odom"));
00048
00049 cost_cloud_.header.frame_id = frame_id_;
00050 pub_.advertise(ns_nh_, "cost_cloud", 1);
00051 }
00052
00053 void MapGridVisualizer::publishCostCloud(const costmap_2d::Costmap2D& costmap_p_) {
00054 unsigned int x_size = costmap_p_.getSizeInCellsX();
00055 unsigned int y_size = costmap_p_.getSizeInCellsY();
00056 double z_coord = 0.0;
00057 double x_coord, y_coord;
00058 MapGridCostPoint pt;
00059 cost_cloud_.points.clear();
00060 cost_cloud_.header.stamp = ros::Time::now();
00061 float path_cost, goal_cost, occ_cost, total_cost;
00062 for (unsigned int cx = 0; cx < x_size; cx++) {
00063 for (unsigned int cy = 0; cy < y_size; cy++) {
00064 costmap_p_.mapToWorld(cx, cy, x_coord, y_coord);
00065 if (cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
00066 pt.x = x_coord;
00067 pt.y = y_coord;
00068 pt.z = z_coord;
00069 pt.path_cost = path_cost;
00070 pt.goal_cost = goal_cost;
00071 pt.occ_cost = occ_cost;
00072 pt.total_cost = total_cost;
00073 cost_cloud_.push_back(pt);
00074 }
00075 }
00076 }
00077 pub_.publish(cost_cloud_);
00078 ROS_DEBUG("Cost PointCloud published");
00079 }
00080 };