costmap_2d_cloud.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009, Willow Garage, Inc.
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  * * Redistributions of source code must retain the above copyright notice,
7  * this list of conditions and the following disclaimer.
8  * * Redistributions in binary form must reproduce the above copyright
9  * notice, this list of conditions and the following disclaimer in the
10  * documentation and/or other materials provided with the distribution.
11  * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
12  * contributors may be used to endorse or promote products derived from
13  * this software without specific prior written permission.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25  * POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 #include <ros/ros.h>
29 #include <sensor_msgs/PointCloud.h>
30 #include <costmap_2d/VoxelGrid.h>
31 #include <voxel_grid/voxel_grid.h>
32 
33 static inline void mapToWorld3D(const unsigned int mx, const unsigned int my, const unsigned int mz,
34  const double origin_x, const double origin_y, const double origin_z,
35  const double x_resolution, const double y_resolution, const double z_resolution,
36  double& wx, double& wy, double& wz)
37 {
38  // returns the center point of the cell
39  wx = origin_x + (mx + 0.5) * x_resolution;
40  wy = origin_y + (my + 0.5) * y_resolution;
41  wz = origin_z + (mz + 0.5) * z_resolution;
42 }
43 
44 struct Cell
45 {
46  double x;
47  double y;
48  double z;
50 };
51 typedef std::vector<Cell> V_Cell;
52 
53 float g_colors_r[] = {0.0f, 0.0f, 1.0f};
54 float g_colors_g[] = {0.0f, 0.0f, 0.0f};
55 float g_colors_b[] = {0.0f, 1.0f, 0.0f};
56 float g_colors_a[] = {0.0f, 0.5f, 1.0f};
57 
60 void voxelCallback(const ros::Publisher& pub_marked, const ros::Publisher& pub_unknown,
61  const costmap_2d::VoxelGridConstPtr& grid)
62 {
63  if (grid->data.empty())
64  {
65  ROS_ERROR("Received empty voxel grid");
66  return;
67  }
68 
70 
71  ROS_DEBUG("Received voxel grid");
72  const std::string frame_id = grid->header.frame_id;
73  const ros::Time stamp = grid->header.stamp;
74  const uint32_t* data = &grid->data.front();
75  const double x_origin = grid->origin.x;
76  const double y_origin = grid->origin.y;
77  const double z_origin = grid->origin.z;
78  const double x_res = grid->resolutions.x;
79  const double y_res = grid->resolutions.y;
80  const double z_res = grid->resolutions.z;
81  const uint32_t x_size = grid->size_x;
82  const uint32_t y_size = grid->size_y;
83  const uint32_t z_size = grid->size_z;
84 
85  g_marked.clear();
86  g_unknown.clear();
87  uint32_t num_marked = 0;
88  uint32_t num_unknown = 0;
89  for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
90  {
91  for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
92  {
93  for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
94  {
95  voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
96  data);
97 
98  if (status == voxel_grid::UNKNOWN)
99  {
100  Cell c;
101  c.status = status;
102  mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res,
103  z_res, c.x, c.y, c.z);
104 
105  g_unknown.push_back(c);
106 
107  ++num_unknown;
108  }
109  else if (status == voxel_grid::MARKED)
110  {
111  Cell c;
112  c.status = status;
113  mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res,
114  z_res, c.x, c.y, c.z);
115 
116  g_marked.push_back(c);
117 
118  ++num_marked;
119  }
120  }
121  }
122  }
123 
124  {
125  sensor_msgs::PointCloud cloud;
126  cloud.points.resize(num_marked);
127  cloud.channels.resize(1);
128  cloud.channels[0].values.resize(num_marked);
129  cloud.channels[0].name = "rgb";
130  cloud.header.frame_id = frame_id;
131  cloud.header.stamp = stamp;
132 
133  sensor_msgs::ChannelFloat32& chan = cloud.channels[0];
134  for (uint32_t i = 0; i < num_marked; ++i)
135  {
136  geometry_msgs::Point32& p = cloud.points[i];
137  float& cval = chan.values[i];
138  Cell& c = g_marked[i];
139 
140  p.x = c.x;
141  p.y = c.y;
142  p.z = c.z;
143 
144  uint32_t r = g_colors_r[c.status] * 255.0;
145  uint32_t g = g_colors_g[c.status] * 255.0;
146  uint32_t b = g_colors_b[c.status] * 255.0;
147  // uint32_t a = g_colors_a[c.status] * 255.0;
148 
149  uint32_t col = (r << 16) | (g << 8) | b;
150  cval = *reinterpret_cast<float*>(&col);
151  }
152 
153  pub_marked.publish(cloud);
154  }
155 
156  {
157  sensor_msgs::PointCloud cloud;
158  cloud.points.resize(num_unknown);
159  cloud.channels.resize(1);
160  cloud.channels[0].values.resize(num_unknown);
161  cloud.channels[0].name = "rgb";
162  cloud.header.frame_id = frame_id;
163  cloud.header.stamp = stamp;
164 
165  sensor_msgs::ChannelFloat32& chan = cloud.channels[0];
166  for (uint32_t i = 0; i < num_unknown; ++i)
167  {
168  geometry_msgs::Point32& p = cloud.points[i];
169  float& cval = chan.values[i];
170  Cell& c = g_unknown[i];
171 
172  p.x = c.x;
173  p.y = c.y;
174  p.z = c.z;
175 
176  uint32_t r = g_colors_r[c.status] * 255.0;
177  uint32_t g = g_colors_g[c.status] * 255.0;
178  uint32_t b = g_colors_b[c.status] * 255.0;
179  // uint32_t a = g_colors_a[c.status] * 255.0;
180 
181  uint32_t col = (r << 16) | (g << 8) | b;
182  cval = *reinterpret_cast<float*>(&col);
183  }
184 
185  pub_unknown.publish(cloud);
186  }
187 
189  ROS_DEBUG("Published %d points in %f seconds", num_marked + num_unknown, (end - start).toSec());
190 }
191 
192 int main(int argc, char** argv)
193 {
194  ros::init(argc, argv, "costmap_2d_cloud");
195  ros::NodeHandle n;
196 
197  ROS_DEBUG("Startup");
198 
199  ros::Publisher pub_marked = n.advertise < sensor_msgs::PointCloud > ("voxel_marked_cloud", 2);
200  ros::Publisher pub_unknown = n.advertise < sensor_msgs::PointCloud > ("voxel_unknown_cloud", 2);
201  ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid
202  > ("voxel_grid", 1, boost::bind(voxelCallback, pub_marked, pub_unknown, _1));
203 
204  ros::spin();
205 }
voxel_grid::VoxelStatus status
void voxelCallback(const ros::Publisher &pub_marked, const ros::Publisher &pub_unknown, const costmap_2d::VoxelGridConstPtr &grid)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< Cell > V_Cell
VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z)
float g_colors_b[]
float g_colors_r[]
ROSCPP_DECL void spin(Spinner &spinner)
V_Cell g_marked
int main(int argc, char **argv)
static void mapToWorld3D(const unsigned int mx, const unsigned int my, const unsigned int mz, const double origin_x, const double origin_y, const double origin_z, const double x_resolution, const double y_resolution, const double z_resolution, double &wx, double &wy, double &wz)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
V_Cell g_unknown
double x
double z
static WallTime now()
float g_colors_g[]
#define ROS_ERROR(...)
#define ROS_DEBUG(...)
float g_colors_a[]
double y


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42