costmap_2d_markers.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  * David V. Lu!!
37  *********************************************************************/
38 
39 #include <ros/ros.h>
40 #include <visualization_msgs/MarkerArray.h>
41 #include <costmap_2d/VoxelGrid.h>
42 #include <voxel_grid/voxel_grid.h>
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 
58 std::string g_marker_ns;
60 void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPtr& grid)
61 {
62  if (grid->data.empty())
63  {
64  ROS_ERROR("Received empty voxel grid");
65  return;
66  }
67 
69 
70  ROS_DEBUG("Received voxel grid");
71  const std::string frame_id = grid->header.frame_id;
72  const ros::Time stamp = grid->header.stamp;
73  const uint32_t* data = &grid->data.front();
74  const double x_origin = grid->origin.x;
75  const double y_origin = grid->origin.y;
76  const double z_origin = grid->origin.z;
77  const double x_res = grid->resolutions.x;
78  const double y_res = grid->resolutions.y;
79  const double z_res = grid->resolutions.z;
80  const uint32_t x_size = grid->size_x;
81  const uint32_t y_size = grid->size_y;
82  const uint32_t z_size = grid->size_z;
83 
84  g_cells.clear();
85  uint32_t num_markers = 0;
86  for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
87  {
88  for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
89  {
90  for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
91  {
92  voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
93  data);
94 
95  if (status == voxel_grid::MARKED)
96  {
97  Cell c;
98  c.status = status;
99  c.x = x_origin + (x_grid + 0.5) * x_res;
100  c.y = y_origin + (y_grid + 0.5) * y_res;
101  c.z = z_origin + (z_grid + 0.5) * z_res;
102  g_cells.push_back(c);
103 
104  ++num_markers;
105  }
106  }
107  }
108  }
109 
110  visualization_msgs::Marker m;
111  m.header.frame_id = frame_id;
112  m.header.stamp = stamp;
113  m.ns = g_marker_ns;
114  m.id = 0;
115  m.type = visualization_msgs::Marker::CUBE_LIST;
116  m.action = visualization_msgs::Marker::ADD;
117  m.pose.orientation.w = 1.0;
118  m.scale.x = x_res;
119  m.scale.y = y_res;
120  m.scale.z = z_res;
121  m.color.r = g_colors_r[voxel_grid::MARKED];
122  m.color.g = g_colors_g[voxel_grid::MARKED];
123  m.color.b = g_colors_b[voxel_grid::MARKED];
124  m.color.a = g_colors_a[voxel_grid::MARKED];
125  m.points.resize(num_markers);
126  for (uint32_t i = 0; i < num_markers; ++i)
127  {
128  Cell& c = g_cells[i];
129  geometry_msgs::Point& p = m.points[i];
130  p.x = c.x;
131  p.y = c.y;
132  p.z = c.z;
133  }
134 
135  pub.publish(m);
136 
138  ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec());
139 }
140 
141 int main(int argc, char** argv)
142 {
143  ros::init(argc, argv, "costmap_2d_markers");
144  ros::NodeHandle n;
145 
146  ROS_DEBUG("Startup");
147 
148  ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1);
149  ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
150  g_marker_ns = n.resolveName("voxel_grid");
151 
152  ros::spin();
153 }
voxel_grid::VoxelStatus status
ROSCPP_DECL void start()
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
void voxelCallback(const ros::Publisher &pub, const costmap_2d::VoxelGridConstPtr &grid)
VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z)
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
std::string resolveName(const std::string &name, bool remap=true) const
std::string g_marker_ns
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
float g_colors_b[]
double x
float g_colors_r[]
std::vector< Cell > V_Cell
double z
V_Cell g_cells
static WallTime now()
float g_colors_g[]
#define ROS_ERROR(...)
float g_colors_a[]
#define ROS_DEBUG(...)
double y


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Jun 22 2022 02:07:03