voxel_layer.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 #include "voxel_layer.h"
41 #include <boost/thread.hpp>
43 
44 #define VOXEL_BITS 16
46 
50 
53 
54 
55 namespace rtabmap_ros
56 {
57 
59 {
60  ObstacleLayer::onInitialize();
61  ros::NodeHandle private_nh("~/" + name_);
62 
63  std::string costmap_name = name_.substr(0, name_.find("/"));
64  ros::NodeHandle pnh("~/" + costmap_name);
65 
66  private_nh.param("publish_voxel_map", publish_voxel_, false);
67  // param from parent costmap group
68  pnh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
69 
70  if (publish_voxel_)
71  voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
72 
73  clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
74 }
75 
77 {
78  voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
79  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
80  &VoxelLayer::reconfigureCB, this, boost::placeholders::_1, boost::placeholders::_2);
81  voxel_dsrv_->setCallback(cb);
82 }
83 
85 {
86  if (voxel_dsrv_)
87  delete voxel_dsrv_;
88 }
89 
90 void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
91 {
92  enabled_ = config.enabled;
93  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
94  max_obstacle_height_ = config.max_obstacle_height;
95  size_z_ = config.z_voxels;
96  origin_z_ = config.origin_z;
97  z_resolution_ = config.z_resolution;
98  unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
99  mark_threshold_ = config.mark_threshold;
100  combination_method_ = config.combination_method;
101  matchSize();
102 }
103 
105 {
106  ObstacleLayer::matchSize();
109 }
110 
112 {
113  deactivate();
114  resetMaps();
115  voxel_grid_.reset();
116  activate();
117 }
118 
120 {
121  Costmap2D::resetMaps();
122  voxel_grid_.reset();
123 }
124 
125 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
126  double* min_y, double* max_x, double* max_y)
127 {
128  if (rolling_window_)
129  updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
130  if (!enabled_)
131  return;
132  useExtraBounds(min_x, min_y, max_x, max_y);
133 
134  bool current = true;
135  std::vector<Observation> observations, clearing_observations;
136 
137  // get the marking observations
138  current = getMarkingObservations(observations) && current;
139 
140  // get the clearing observations
141  current = getClearingObservations(clearing_observations) && current;
142 
143  // update the global current status
144  current_ = current;
145 
146  // raytrace freespace
147  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
148  {
149  raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
150  }
151 
152  // place the new obstacles into a priority queue... each with a priority of zero to begin with
153  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
154  {
155  const Observation& obs = *it;
156 
157 #ifdef COSTMAP_2D_POINTCLOUD2
158  const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
159  // Note that these points are already filtered with max and min obstacle heights
160  // You will have to adjust param file to see points beyond default settings
164 
165  for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
166  const double x = *iter_x;
167  const double y = *iter_y;
168  const double z = *iter_z;
169 #else
170  pcl::PointCloud<pcl::PointXYZ>::const_iterator point_it = obs.cloud_->begin();
171  for (;point_it < obs.cloud_->end(); ++point_it) {
172  const double x = point_it->x;
173  const double y = point_it->y;
174  const double z = point_it->z;
175 #endif
176  // now we need to compute the map coordinates for the observation
177  unsigned int mx, my, mz;
178 
179  if (!worldToMap3D(x, y, z, mx, my, mz))
180  continue;
181 
182  if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
183  {
184  unsigned int index = getIndex(mx, my);
185 
186  costmap_[index] = LETHAL_OBSTACLE;
187  touch(x, y, min_x, min_y, max_x, max_y);
188  }
189  }
190  }
191 
192  if (publish_voxel_)
193  {
194  costmap_2d::VoxelGrid grid_msg;
195  unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
196  grid_msg.size_x = voxel_grid_.sizeX();
197  grid_msg.size_y = voxel_grid_.sizeY();
198  grid_msg.size_z = voxel_grid_.sizeZ();
199  grid_msg.data.resize(size);
200  memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
201 
202  grid_msg.origin.x = origin_x_;
203  grid_msg.origin.y = origin_y_;
204  grid_msg.origin.z = origin_z_;
205 
206  grid_msg.resolutions.x = resolution_;
207  grid_msg.resolutions.y = resolution_;
208  grid_msg.resolutions.z = z_resolution_;
209  grid_msg.header.frame_id = global_frame_;
210  grid_msg.header.stamp = ros::Time::now();
211  voxel_pub_.publish(grid_msg);
212  }
213 
214  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
215 }
216 
217 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
218 {
219  // get the cell coordinates of the center point of the window
220  unsigned int mx, my;
221  if (!worldToMap(wx, wy, mx, my))
222  return;
223 
224  // compute the bounds of the window
225  double start_x = wx - w_size_x / 2;
226  double start_y = wy - w_size_y / 2;
227  double end_x = start_x + w_size_x;
228  double end_y = start_y + w_size_y;
229 
230  // scale the window based on the bounds of the costmap
231  start_x = std::max(origin_x_, start_x);
232  start_y = std::max(origin_y_, start_y);
233 
234  end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
235  end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
236 
237  // get the map coordinates of the bounds of the window
238  unsigned int map_sx, map_sy, map_ex, map_ey;
239 
240  // check for legality just in case
241  if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
242  return;
243 
244  // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
245  unsigned int index = getIndex(map_sx, map_sy);
246  unsigned char* current = &costmap_[index];
247  for (unsigned int j = map_sy; j <= map_ey; ++j)
248  {
249  for (unsigned int i = map_sx; i <= map_ex; ++i)
250  {
251  // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
252  if (*current != LETHAL_OBSTACLE)
253  {
254  if (clear_no_info || *current != NO_INFORMATION)
255  {
256  *current = FREE_SPACE;
258  }
259  }
260  current++;
261  index++;
262  }
263  current += size_x_ - (map_ex - map_sx) - 1;
264  index += size_x_ - (map_ex - map_sx) - 1;
265  }
266 }
267 
268 void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
269  double* max_x, double* max_y)
270 {
271  size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
272  if (clearing_observation_cloud_size == 0)
273  return;
274 
275  double sensor_x, sensor_y, sensor_z;
276  double ox = clearing_observation.origin_.x;
277  double oy = clearing_observation.origin_.y;
278  double oz = clearing_observation.origin_.z;
279 
280  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
281  {
283  1.0,
284  "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
285  ox, oy, oz);
286  return;
287  }
288 
289  bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
290  if (publish_clearing_points)
291  {
292  clearing_endpoints_.points.clear();
293  clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
294  }
295 
296  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
297  double map_end_x = origin_x_ + getSizeInMetersX();
298  double map_end_y = origin_y_ + getSizeInMetersY();
299  double map_end_z = origin_z_ + size_z_ * z_resolution_;
300 
301 #ifdef COSTMAP_2D_POINTCLOUD2
302  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
303  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
304  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
305 
306  for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
307  {
308  double wpx = *iter_x;
309  double wpy = *iter_y;
310  double wpz = *iter_z;
311 #else
312  pcl::PointCloud<pcl::PointXYZ>::const_iterator point_it = clearing_observation.cloud_->begin();
313  for (;point_it < clearing_observation.cloud_->end(); ++point_it) {
314  double wpx = point_it->x;
315  double wpy = point_it->y;
316  double wpz = point_it->z;
317 #endif
318 
319  double distance = dist(ox, oy, oz, wpx, wpy, wpz);
320  double scaling_fact = 1.0, scaling_fact_z = 1.0;
321  scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
322  scaling_fact_z = std::max(std::min(scaling_fact_z, (distance - 2 * z_resolution_) / distance), 0.0);
323  wpx = scaling_fact * (wpx - ox) + ox;
324  wpy = scaling_fact * (wpy - oy) + oy;
325  wpz = scaling_fact_z * (wpz - oz) + oz;
326 
327  double a = wpx - ox;
328  double b = wpy - oy;
329  double c = wpz - oz;
330  double t = 1.0;
331 
332  // the minimum value to raytrace from is the origin
333  if (wpz < origin_z_)
334  {
335  t = std::min(t, (origin_z_ - oz) / c);
336  }
337  if (wpx < origin_x_)
338  {
339  t = std::min(t, (origin_x_ - ox) / a);
340  }
341  if (wpy < origin_y_)
342  {
343  t = std::min(t, (origin_y_ - oy) / b);
344  }
345 
346  // the maximum value to raytrace to is the end of the map
347  if (wpx > map_end_x)
348  {
349  t = std::min(t, (map_end_x - ox) / a);
350  }
351  if (wpy > map_end_y)
352  {
353  t = std::min(t, (map_end_y - oy) / b);
354  }
355  if (wpz > map_end_z)
356  {
357  t = std::min(t, (map_end_z - oz) / c);
358  }
359 
360  wpx = ox + a * t;
361  wpy = oy + b * t;
362  wpz = oz + c * t;
363 
364  double point_x, point_y, point_z;
365  if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
366  {
367  unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
368 
369  // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
370  voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
371  unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
372  cell_raytrace_range);
373 
374  updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
375 
376  if (publish_clearing_points)
377  {
378  geometry_msgs::Point32 point;
379  point.x = wpx;
380  point.y = wpy;
381  point.z = wpz;
382  clearing_endpoints_.points.push_back(point);
383  }
384  }
385  }
386 
387  if (publish_clearing_points)
388  {
389  clearing_endpoints_.header.frame_id = global_frame_;
390 #ifdef COSTMAP_2D_POINTCLOUD2
391  clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
392 #else
393  clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header.stamp);
394 #endif
395  clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
396 
398  }
399 }
400 
401 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
402 {
403  int cell_oz;
404  // get the global pose of the robot
405  try
406  {
407  geometry_msgs::TransformStamped transformStamped;
408 #ifdef COSTMAP_2D_POINTCLOUD2
409  transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0));
410 #else
411  tf::StampedTransform stampedTransform;
413  tf::transformStampedTFToMsg(stampedTransform, transformStamped);
414 #endif
415  const double robot_z = transformStamped.transform.translation.z;
416  const double z_grid_height = z_resolution_ * size_z_;
417  const double new_origin_z = robot_z - z_grid_height / 2;
418  cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
419  }
420 #ifdef COSTMAP_2D_POINTCLOUD2
421  catch (tf2::TransformException& ex)
422 #else
423  catch(tf::TransformException& ex)
424 #endif
425  {
426  ROS_ERROR("%s", ex.what());
427  // If the robot pose is not detected, the origin_z_ will remain the same.
428  cell_oz = 0;
429  }
430 
431  // project the new origin into the grid
432  int cell_ox, cell_oy;
433  cell_ox = int((new_origin_x - origin_x_) / resolution_);
434  cell_oy = int((new_origin_y - origin_y_) / resolution_);
435 
436  // compute the associated world coordinates for the origin cell
437  // because we want to keep things grid-aligned
438  double new_grid_ox, new_grid_oy, new_grid_oz;
439  new_grid_ox = origin_x_ + cell_ox * resolution_;
440  new_grid_oy = origin_y_ + cell_oy * resolution_;
441  new_grid_oz = origin_z_ + cell_oz * z_resolution_;
442 
443  // to avoid casting from unsigned int to int a bunch of times
444  int size_x = size_x_;
445  int size_y = size_y_;
446 
447  // we need to compute the overlap of the new and existing windows
448  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
449  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
450  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
451  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
452  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
453 
454  unsigned int cell_size_x = upper_right_x - lower_left_x;
455  unsigned int cell_size_y = upper_right_y - lower_left_y;
456 
457  // we need a map to store the obstacles in the window temporarily
458  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
459  unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
460  unsigned int* voxel_map = voxel_grid_.getData();
461 
462  // copy the local window in the costmap to the local map
463  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
464  copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
465  cell_size_y);
466 
467  // we'll reset our maps to unknown space if appropriate
468  resetMaps();
469 
470  // update the origin with the appropriate world coordinates
471  origin_x_ = new_grid_ox;
472  origin_y_ = new_grid_oy;
473  origin_z_ = new_grid_oz;
474 
475  // compute the starting cell location for copying data back in
476  int start_x = lower_left_x - cell_ox;
477  int start_y = lower_left_y - cell_oy;
478 
479  // now we want to copy the overlapping information back into the map, but in its new location
480  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
481  copyMapRegion3D(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y, cell_oz);
482 
483  // make sure to clean up
484  delete[] local_map;
485  delete[] local_voxel_map;
486 }
487 
488 } // namespace rtabmap_ros
double getSizeInMetersX() const
geometry_msgs::Point origin_
bool worldToMap3DFloat(double wx, double wy, double wz, double &mx, double &my, double &mz)
Definition: voxel_layer.h:106
dynamic_reconfigure::Server< costmap_2d::VoxelPluginConfig > * voxel_dsrv_
Definition: voxel_layer.h:95
bool getMarkingObservations(std::vector< costmap_2d::Observation > &marking_observations) const
void copyMapRegion3D(data_type *source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type *dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y, int z_shift)
Copy a region of a source map into a destination map.
Definition: voxel_layer.h:162
ros::Publisher voxel_pub_
Definition: voxel_layer.h:99
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
unsigned int cellDistance(double world_dist)
unsigned int size_y_
virtual void deactivate()
x
virtual void matchSize()
bool worldToMap3D(double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
Definition: voxel_layer.h:119
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
std::string robot_base_frame_
Definition: voxel_layer.h:98
voxel_grid::VoxelGrid voxel_grid_
Definition: voxel_layer.h:100
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double *min_x, double *min_y, double *max_x, double *max_y)
bool markVoxelInMap(unsigned int x, unsigned int y, unsigned int z, unsigned int marked_threshold)
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
virtual void raytraceFreespace(const costmap_2d::Observation &clearing_observation, double *min_x, double *min_y, double *max_x, double *max_y)
unsigned int getIndex(unsigned int mx, unsigned int my) const
unsigned int size_x_
virtual void resetMaps()
static const unsigned char FREE_SPACE
double getSizeInMetersY() const
geometry_msgs::TransformStamped t
void useExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y)
void clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d, unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost=0, unsigned char unknown_cost=255, unsigned int max_length=UINT_MAX)
std::string name_
void copyMapRegion(data_type *source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type *dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
virtual void setupDynamicReconfigure(ros::NodeHandle &nh)
Definition: voxel_layer.cpp:76
tf2_ros::Buffer * tf_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
void publish(const boost::shared_ptr< M > &message) const
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
Definition: voxel_layer.h:142
unsigned int sizeY()
unsigned int mark_threshold_
Definition: voxel_layer.h:102
#define ROS_WARN_THROTTLE(period,...)
virtual void onInitialize()
Definition: voxel_layer.cpp:58
unsigned int sizeX()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
unsigned int unknown_threshold_
Definition: voxel_layer.h:102
void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
Definition: voxel_layer.cpp:90
detail::uint32 uint32_t
sensor_msgs::PointCloud clearing_endpoints_
Definition: voxel_layer.h:104
ros::Publisher clearing_endpoints_pub_
Definition: voxel_layer.h:103
uint32_t * getData()
sensor_msgs::PointCloud2 * cloud_
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
void clearVoxelColumn(unsigned int index)
static Time now()
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
void touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y)
#define ROS_ASSERT(cond)
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
uint32_t getNumSubscribers() const
unsigned char * costmap_
#define ROS_ERROR(...)
unsigned int sizeZ()
bool getClearingObservations(std::vector< costmap_2d::Observation > &clearing_observations) const
PLUGINLIB_EXPORT_CLASS(apriltag_ros::ContinuousDetector, nodelet::Nodelet)
#define VOXEL_BITS
Definition: voxel_layer.cpp:44
void updateOrigin(double new_origin_x, double new_origin_y)


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40