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
00035
00036
00037
00038
00039 #include <ros/ros.h>
00040 #include <visualization_msgs/MarkerArray.h>
00041 #include <costmap_2d/VoxelGrid.h>
00042 #include <voxel_grid/voxel_grid.h>
00043
00044 struct Cell
00045 {
00046 double x;
00047 double y;
00048 double z;
00049 voxel_grid::VoxelStatus status;
00050 };
00051 typedef std::vector<Cell> V_Cell;
00052
00053 float g_colors_r[] = {0.0f, 0.0f, 1.0f};
00054 float g_colors_g[] = {0.0f, 0.0f, 0.0f};
00055 float g_colors_b[] = {0.0f, 1.0f, 0.0f};
00056 float g_colors_a[] = {0.0f, 0.5f, 1.0f};
00057
00058 std::string g_marker_ns;
00059 V_Cell g_cells;
00060 void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPtr& grid)
00061 {
00062 if (grid->data.empty())
00063 {
00064 ROS_ERROR("Received empty voxel grid");
00065 return;
00066 }
00067
00068 ros::WallTime start = ros::WallTime::now();
00069
00070 ROS_DEBUG("Received voxel grid");
00071 const std::string frame_id = grid->header.frame_id;
00072 const ros::Time stamp = grid->header.stamp;
00073 const uint32_t* data = &grid->data.front();
00074 const double x_origin = grid->origin.x;
00075 const double y_origin = grid->origin.y;
00076 const double z_origin = grid->origin.z;
00077 const double x_res = grid->resolutions.x;
00078 const double y_res = grid->resolutions.y;
00079 const double z_res = grid->resolutions.z;
00080 const uint32_t x_size = grid->size_x;
00081 const uint32_t y_size = grid->size_y;
00082 const uint32_t z_size = grid->size_z;
00083
00084 g_cells.clear();
00085 uint32_t num_markers = 0;
00086 for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
00087 {
00088 for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
00089 {
00090 for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
00091 {
00092 voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
00093 data);
00094
00095 if (status == voxel_grid::MARKED)
00096 {
00097 Cell c;
00098 c.status = status;
00099 c.x = x_origin + (x_grid + 0.5) * x_res;
00100 c.y = y_origin + (y_grid + 0.5) * y_res;
00101 c.z = z_origin + (z_grid + 0.5) * z_res;
00102 g_cells.push_back(c);
00103
00104 ++num_markers;
00105 }
00106 }
00107 }
00108 }
00109
00110 visualization_msgs::Marker m;
00111 m.header.frame_id = frame_id;
00112 m.header.stamp = stamp;
00113 m.ns = g_marker_ns;
00114 m.id = 0;
00115 m.type = visualization_msgs::Marker::CUBE_LIST;
00116 m.action = visualization_msgs::Marker::ADD;
00117 m.pose.orientation.w = 1.0;
00118 m.scale.x = x_res;
00119 m.scale.y = y_res;
00120 m.scale.z = z_res;
00121 m.color.r = g_colors_r[voxel_grid::MARKED];
00122 m.color.g = g_colors_g[voxel_grid::MARKED];
00123 m.color.b = g_colors_b[voxel_grid::MARKED];
00124 m.color.a = g_colors_a[voxel_grid::MARKED];
00125 m.points.resize(num_markers);
00126 for (uint32_t i = 0; i < num_markers; ++i)
00127 {
00128 Cell& c = g_cells[i];
00129 geometry_msgs::Point& p = m.points[i];
00130 p.x = c.x;
00131 p.y = c.y;
00132 p.z = c.z;
00133 }
00134
00135 pub.publish(m);
00136
00137 ros::WallTime end = ros::WallTime::now();
00138 ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec());
00139 }
00140
00141 int main(int argc, char** argv)
00142 {
00143 ros::init(argc, argv, "costmap_2d_markers");
00144 ros::NodeHandle n;
00145
00146 ROS_DEBUG("Startup");
00147
00148 ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1);
00149 ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
00150 g_marker_ns = n.resolveName("voxel_grid");
00151
00152 ros::spin();
00153 }
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15