voxel_layer.h
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 #ifndef RTABMAP_ROS_VOXEL_LAYER_H_
39 #define RTABMAP_ROS_VOXEL_LAYER_H_
40 
41 #include <costmap_2d/layer.h>
44 #include <costmap_2d/VoxelGrid.h>
45 #include <nav_msgs/OccupancyGrid.h>
46 #include <sensor_msgs/LaserScan.h>
48 #include <sensor_msgs/PointCloud.h>
49 #include <sensor_msgs/PointCloud2.h>
52 #include <dynamic_reconfigure/server.h>
53 #include <costmap_2d/VoxelPluginConfig.h>
55 #include <voxel_grid/voxel_grid.h>
56 
57 namespace rtabmap_ros
58 {
59 
61 {
62 public:
64  voxel_grid_(0, 0, 0)
65  {
66  costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
67  }
68 
69  virtual ~VoxelLayer();
70 
71  virtual void onInitialize();
72  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
73  double* max_x, double* max_y);
74 
75  void updateOrigin(double new_origin_x, double new_origin_y);
77  {
78  return true;
79  }
80  virtual void matchSize();
81  virtual void reset();
82 
83 
84 protected:
85  virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
86 
87  virtual void resetMaps();
88 
89 private:
90  void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level);
91  void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
92  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
93  double* max_x, double* max_y);
94 
95  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
96 
98  std::string robot_base_frame_;
104  sensor_msgs::PointCloud clearing_endpoints_;
105 
106  inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
107  {
108  if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
109  return false;
110  mx = ((wx - origin_x_) / resolution_);
111  my = ((wy - origin_y_) / resolution_);
112  mz = ((wz - origin_z_) / z_resolution_);
113  if (mx < size_x_ && my < size_y_ && mz < size_z_)
114  return true;
115 
116  return false;
117  }
118 
119  inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz)
120  {
121  if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
122  return false;
123 
124  mx = (int)((wx - origin_x_) / resolution_);
125  my = (int)((wy - origin_y_) / resolution_);
126  mz = (int)((wz - origin_z_) / z_resolution_);
127 
128  if (mx < size_x_ && my < size_y_ && mz < size_z_)
129  return true;
130 
131  return false;
132  }
133 
134  inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz)
135  {
136  // returns the center point of the cell
137  wx = origin_x_ + (mx + 0.5) * resolution_;
138  wy = origin_y_ + (my + 0.5) * resolution_;
139  wz = origin_z_ + (mz + 0.5) * z_resolution_;
140  }
141 
142  inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
143  {
144  return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
145  }
146 
147 
161  template<typename data_type>
162  void copyMapRegion3D(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
163  unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
164  unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
165  unsigned int region_size_y, int z_shift)
166  {
167  // we'll first need to compute the starting points for each map
168  // this is like getting voxel column. We are not taking into account the z position of the voxel
169  data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
170  data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
171 
172  uint32_t marked_bits_mask = (data_type) 0xFFFF0000;
173  uint32_t unknown_bits_mask = (data_type) 0x0000FFFF;
174 
175  // now, we'll copy the source map into the destination map
176  for (unsigned int i = 0; i < region_size_y; ++i)
177  {
178  memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
179  for (unsigned int j = 0; j < region_size_x; j++) {
180  // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
181  if (z_shift > 0) {
182  dm_index[j] =
183  // Shift marked cells, insert zeros for new unknowns
184  ((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
185  // Shift empty/unknown cells, insert ones for new unknowns
186  (((dm_index[j] & unknown_bits_mask) >> z_shift | (~((data_type) 0) << sizeof(data_type) * 4 - z_shift)) & unknown_bits_mask);
187 
188  } else if (z_shift < 0) {
189  dm_index[j] =
190  // Shift marked cells, insert zeros for new unknowns
191  (dm_index[j] & marked_bits_mask) << z_shift * -1 |
192  // Shift empty/unknown cells, insert ones for new unknowns
193  ((dm_index[j] << z_shift * -1 & unknown_bits_mask) | ~(~((data_type) 0) << z_shift * -1));
194  }
195  }
196 
197  dm_index += dm_size_x;
198  sm_index += sm_size_x;
199  }
200  }
201 };
202 
203 } // namespace rtabmap_ros
204 
205 #endif // RTABMAP_ROS_VOXEL_LAYER_H_
#define NULL
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
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)
unsigned int size_y_
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
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
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
config
virtual void raytraceFreespace(const costmap_2d::Observation &clearing_observation, double *min_x, double *min_y, double *max_x, double *max_y)
unsigned int size_x_
virtual void resetMaps()
virtual void setupDynamicReconfigure(ros::NodeHandle &nh)
Definition: voxel_layer.cpp:76
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
Definition: voxel_layer.h:142
unsigned int mark_threshold_
Definition: voxel_layer.h:102
virtual void onInitialize()
Definition: voxel_layer.cpp:58
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
void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
Definition: voxel_layer.h:134
sensor_msgs::PointCloud clearing_endpoints_
Definition: voxel_layer.h:104
ros::Publisher clearing_endpoints_pub_
Definition: voxel_layer.h:103
unsigned char * costmap_
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