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 #ifndef EXPLORATION_TRANSFORM_VIS_H___
00030 #define EXPLORATION_TRANSFORM_VIS_H___
00031
00032 #include <sensor_msgs/PointCloud.h>
00033 #include <ros/ros.h>
00034 #include <costmap_2d/costmap_2d.h>
00035
00036
00037 namespace hector_exploration_planner{
00038
00039 class ExplorationTransformVis
00040 {
00041 public:
00042 ExplorationTransformVis(const std::string& topic_name)
00043 {
00044 ros::NodeHandle pnh("~");
00045 exploration_transform_pointcloud_pub_ = pnh.advertise<sensor_msgs::PointCloud>(topic_name, 2, false);
00046 }
00047
00048 virtual ~ExplorationTransformVis()
00049 {}
00050
00051 void publishVisOnDemand(const costmap_2d::Costmap2D& costmap, const unsigned int* exploration_array)
00052 {
00053 if (exploration_transform_pointcloud_pub_.getNumSubscribers() > 0){
00054 unsigned int size_x = costmap.getSizeInCellsX();
00055 unsigned int size_y = costmap.getSizeInCellsY();
00056 unsigned int size = size_x * size_y;
00057
00058 unsigned int max = 0;
00059
00060 for (size_t i = 0; i < size; ++i){
00061 if ((exploration_array[i] < INT_MAX) && (exploration_array[i] > max)){
00062 max = exploration_array[i];
00063 }
00064 }
00065
00066 float max_f = static_cast<float>(max);
00067
00068 sensor_msgs::PointCloud cloud;
00069 cloud.header.frame_id = "/map";
00070 cloud.header.stamp = ros::Time::now();
00071
00072 double x_world, y_world;
00073
00074 geometry_msgs::Point32 point;
00075
00076 for (size_t x = 0; x < size_x; ++x){
00077 for (size_t y = 0; y < size_y; ++y){
00078
00079 unsigned int index = costmap.getIndex(x,y);
00080
00081 if (exploration_array[index] < INT_MAX){
00082
00083 costmap.mapToWorld(x,y, x_world, y_world);
00084 point.x = x_world;
00085 point.y = y_world;
00086 point.z = static_cast<float>(exploration_array[index])/max_f;
00087
00088 cloud.points.push_back(point);
00089 }
00090
00091 }
00092 }
00093 exploration_transform_pointcloud_pub_.publish(cloud);
00094 }
00095 }
00096
00097 protected:
00098
00099 ros::Publisher exploration_transform_pointcloud_pub_;
00100 };
00101
00102 }
00103
00104 #endif