$search
00001 /* 00002 * Copyright (C) 2009, Willow Garage, Inc. 00003 * 00004 * Redistribution and use in source and binary forms, with or without 00005 * modification, are permitted provided that the following conditions are met: 00006 * * Redistributions of source code must retain the above copyright notice, 00007 * this list of conditions and the following disclaimer. 00008 * * Redistributions in binary form must reproduce the above copyright 00009 * notice, this list of conditions and the following disclaimer in the 00010 * documentation and/or other materials provided with the distribution. 00011 * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 00012 * contributors may be used to endorse or promote products derived from 00013 * this software without specific prior written permission. 00014 * 00015 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00016 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00017 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00018 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00019 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00020 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00021 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00022 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00023 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00024 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00025 * POSSIBILITY OF SUCH DAMAGE. 00026 */ 00027 00028 #include <ros/ros.h> 00029 #include <sensor_msgs/PointCloud.h> 00030 #include <costmap_2d/VoxelGrid.h> 00031 #include <voxel_grid/voxel_grid.h> 00032 00033 #include "costmap_2d/voxel_costmap_2d.h" 00034 00035 struct Cell 00036 { 00037 double x; 00038 double y; 00039 double z; 00040 voxel_grid::VoxelStatus status; 00041 }; 00042 typedef std::vector<Cell> V_Cell; 00043 00044 float g_colors_r[] = { 0.0f, 0.0f, 1.0f }; 00045 float g_colors_g[] = { 0.0f, 0.0f, 0.0f }; 00046 float g_colors_b[] = { 0.0f, 1.0f, 0.0f }; 00047 float g_colors_a[] = { 0.0f, 0.5f, 1.0f }; 00048 00049 V_Cell g_marked; 00050 V_Cell g_unknown; 00051 void voxelCallback(const ros::Publisher& pub_marked, const ros::Publisher& pub_unknown, const costmap_2d::VoxelGridConstPtr& grid) 00052 { 00053 if (grid->data.empty()) 00054 { 00055 ROS_ERROR("Received empty voxel grid"); 00056 return; 00057 } 00058 00059 ros::WallTime start = ros::WallTime::now(); 00060 00061 ROS_DEBUG("Received voxel grid"); 00062 const std::string frame_id = grid->header.frame_id; 00063 const ros::Time stamp = grid->header.stamp; 00064 const uint32_t* data = &grid->data.front(); 00065 const double x_origin = grid->origin.x; 00066 const double y_origin = grid->origin.y; 00067 const double z_origin = grid->origin.z; 00068 const double x_res = grid->resolutions.x; 00069 const double y_res = grid->resolutions.y; 00070 const double z_res = grid->resolutions.z; 00071 const uint32_t x_size = grid->size_x; 00072 const uint32_t y_size = grid->size_y; 00073 const uint32_t z_size = grid->size_z; 00074 00075 g_marked.clear(); 00076 g_unknown.clear(); 00077 uint32_t num_marked = 0; 00078 uint32_t num_unknown = 0; 00079 for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) 00080 { 00081 for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) 00082 { 00083 for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) 00084 { 00085 voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, data); 00086 00087 if (status == voxel_grid::UNKNOWN) 00088 { 00089 Cell c; 00090 c.status = status; 00091 costmap_2d::VoxelCostmap2D::mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res, z_res, c.x, c.y, c.z); 00092 00093 g_unknown.push_back(c); 00094 00095 ++num_unknown; 00096 } 00097 else if (status == voxel_grid::MARKED) 00098 { 00099 Cell c; 00100 c.status = status; 00101 costmap_2d::VoxelCostmap2D::mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res, z_res, c.x, c.y, c.z); 00102 00103 g_marked.push_back(c); 00104 00105 ++num_marked; 00106 } 00107 00108 } 00109 } 00110 } 00111 00112 { 00113 sensor_msgs::PointCloud cloud; 00114 cloud.points.resize(num_marked); 00115 cloud.channels.resize(1); 00116 cloud.channels[0].values.resize(num_marked); 00117 cloud.channels[0].name = "rgb"; 00118 cloud.header.frame_id = frame_id; 00119 cloud.header.stamp = stamp; 00120 00121 sensor_msgs::ChannelFloat32& chan = cloud.channels[0]; 00122 for (uint32_t i = 0; i < num_marked; ++i) 00123 { 00124 geometry_msgs::Point32& p = cloud.points[i]; 00125 float& cval = chan.values[i]; 00126 Cell& c = g_marked[i]; 00127 00128 p.x = c.x; 00129 p.y = c.y; 00130 p.z = c.z; 00131 00132 uint32_t r = g_colors_r[c.status] * 255.0; 00133 uint32_t g = g_colors_g[c.status] * 255.0; 00134 uint32_t b = g_colors_b[c.status] * 255.0; 00135 //uint32_t a = g_colors_a[c.status] * 255.0; 00136 00137 uint32_t col = (r << 16) | (g << 8) | b; 00138 cval = *reinterpret_cast<float*>(&col); 00139 } 00140 00141 pub_marked.publish(cloud); 00142 } 00143 00144 { 00145 sensor_msgs::PointCloud cloud; 00146 cloud.points.resize(num_unknown); 00147 cloud.channels.resize(1); 00148 cloud.channels[0].values.resize(num_unknown); 00149 cloud.channels[0].name = "rgb"; 00150 cloud.header.frame_id = frame_id; 00151 cloud.header.stamp = stamp; 00152 00153 sensor_msgs::ChannelFloat32& chan = cloud.channels[0]; 00154 for (uint32_t i = 0; i < num_unknown; ++i) 00155 { 00156 geometry_msgs::Point32& p = cloud.points[i]; 00157 float& cval = chan.values[i]; 00158 Cell& c = g_unknown[i]; 00159 00160 p.x = c.x; 00161 p.y = c.y; 00162 p.z = c.z; 00163 00164 uint32_t r = g_colors_r[c.status] * 255.0; 00165 uint32_t g = g_colors_g[c.status] * 255.0; 00166 uint32_t b = g_colors_b[c.status] * 255.0; 00167 //uint32_t a = g_colors_a[c.status] * 255.0; 00168 00169 uint32_t col = (r << 16) | (g << 8) | b; 00170 cval = *reinterpret_cast<float*>(&col); 00171 } 00172 00173 pub_unknown.publish(cloud); 00174 } 00175 00176 ros::WallTime end = ros::WallTime::now(); 00177 ROS_DEBUG("Published %d points in %f seconds", num_marked + num_unknown, (end - start).toSec()); 00178 } 00179 00180 int main(int argc, char** argv) 00181 { 00182 ros::init(argc, argv, "costmap_2d_cloud"); 00183 ros::NodeHandle n; 00184 00185 ROS_DEBUG("Startup"); 00186 00187 ros::Publisher pub_marked = n.advertise<sensor_msgs::PointCloud>("voxel_marked_cloud", 2); 00188 ros::Publisher pub_unknown = n.advertise<sensor_msgs::PointCloud>("voxel_unknown_cloud", 2); 00189 ros::Subscriber sub = n.subscribe<costmap_2d::VoxelGrid>("voxel_grid", 1, boost::bind(voxelCallback, pub_marked, pub_unknown, _1)); 00190 00191 ros::spin(); 00192 }