$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 <visualization_msgs/MarkerArray.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 std::string g_marker_ns; 00050 V_Cell g_cells; 00051 void voxelCallback(const ros::Publisher& pub, 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_cells.clear(); 00076 uint32_t num_markers = 0; 00077 for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) 00078 { 00079 for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) 00080 { 00081 for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) 00082 { 00083 voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, data); 00084 00085 if (status == voxel_grid::MARKED) 00086 { 00087 Cell c; 00088 c.status = status; 00089 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); 00090 00091 g_cells.push_back(c); 00092 00093 ++num_markers; 00094 } 00095 } 00096 } 00097 } 00098 00099 visualization_msgs::Marker m; 00100 m.header.frame_id = frame_id; 00101 m.header.stamp = stamp; 00102 m.ns = g_marker_ns; 00103 m.id = 0; 00104 m.type = visualization_msgs::Marker::CUBE_LIST; 00105 m.action = visualization_msgs::Marker::ADD; 00106 m.pose.orientation.w = 1.0; 00107 m.scale.x = x_res; 00108 m.scale.y = y_res; 00109 m.scale.z = z_res; 00110 m.color.r = g_colors_r[voxel_grid::MARKED]; 00111 m.color.g = g_colors_g[voxel_grid::MARKED]; 00112 m.color.b = g_colors_b[voxel_grid::MARKED]; 00113 m.color.a = g_colors_a[voxel_grid::MARKED]; 00114 m.points.resize(num_markers); 00115 for (uint32_t i = 0; i < num_markers; ++i) 00116 { 00117 Cell& c = g_cells[i]; 00118 geometry_msgs::Point& p = m.points[i]; 00119 p.x = c.x; 00120 p.y = c.y; 00121 p.z = c.z; 00122 } 00123 00124 pub.publish(m); 00125 00126 ros::WallTime end = ros::WallTime::now(); 00127 ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec()); 00128 } 00129 00130 int main(int argc, char** argv) 00131 { 00132 ros::init(argc, argv, "costmap_2d_markers"); 00133 ros::NodeHandle n; 00134 00135 ROS_DEBUG("Startup"); 00136 00137 ros::Publisher pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1); 00138 ros::Subscriber sub = n.subscribe<costmap_2d::VoxelGrid>("voxel_grid", 1, boost::bind(voxelCallback, pub, _1)); 00139 g_marker_ns = n.resolveName("voxel_grid"); 00140 00141 ros::spin(); 00142 }