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 COSTMAP_2D_VOXEL_LAYER_H_
39 #define COSTMAP_2D_VOXEL_LAYER_H_
40 
41 #include <ros/ros.h>
42 #include <costmap_2d/layer.h>
45 #include <costmap_2d/VoxelGrid.h>
46 #include <nav_msgs/OccupancyGrid.h>
47 #include <sensor_msgs/LaserScan.h>
49 #include <sensor_msgs/PointCloud.h>
50 #include <sensor_msgs/PointCloud2.h>
53 #include <dynamic_reconfigure/server.h>
54 #include <costmap_2d/VoxelPluginConfig.h>
56 #include <voxel_grid/voxel_grid.h>
57 
58 namespace costmap_2d
59 {
60 
61 class VoxelLayer : public ObstacleLayer
62 {
63 public:
64  VoxelLayer() :
65  voxel_grid_(0, 0, 0)
66  {
67  costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
68  }
69 
70  virtual ~VoxelLayer();
71 
72  virtual void onInitialize();
73  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
74  double* max_x, double* max_y);
75 
76  void updateOrigin(double new_origin_x, double new_origin_y);
77  bool isDiscretized()
78  {
79  return true;
80  }
81  virtual void matchSize();
82  virtual void reset();
83 
84 
85 protected:
86  virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
87 
88  virtual void resetMaps();
89 
90 private:
91  void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level);
92  void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
93  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
94  double* max_x, double* max_y);
95 
96  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
97 
98  bool publish_voxel_;
101  double z_resolution_, origin_z_;
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 
148 } // namespace costmap_2d
149 
150 #endif // COSTMAP_2D_VOXEL_LAYER_H_
costmap_2d::VoxelLayer::origin_z_
double origin_z_
Definition: voxel_layer.h:173
costmap_2d::VoxelLayer::reconfigureCB
void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
Definition: voxel_layer.cpp:81
obstacle_layer.h
ros::Publisher
costmap_2d::VoxelLayer::raytraceFreespace
virtual void raytraceFreespace(const costmap_2d::Observation &clearing_observation, double *min_x, double *min_y, double *max_x, double *max_y)
Clear freespace based on one observation.
Definition: voxel_layer.cpp:268
costmap_2d::VoxelLayer::isDiscretized
bool isDiscretized()
Definition: voxel_layer.h:149
costmap_2d::VoxelLayer::resetMaps
virtual void resetMaps()
Resets the costmap and static_map to be unknown space.
Definition: voxel_layer.cpp:110
ros.h
costmap_2d::VoxelLayer::dist
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
Definition: voxel_layer.h:214
costmap_2d::VoxelLayer::worldToMap3DFloat
bool worldToMap3DFloat(double wx, double wy, double wz, double &mx, double &my, double &mz)
Definition: voxel_layer.h:178
voxel_grid.h
costmap_2d::VoxelLayer::voxel_grid_
voxel_grid::VoxelGrid voxel_grid_
Definition: voxel_layer.h:172
costmap_2d::Costmap2D::origin_x_
double origin_x_
Definition: costmap_2d.h:460
layered_costmap.h
layer.h
costmap_2d::VoxelLayer::worldToMap3D
bool worldToMap3D(double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
Definition: voxel_layer.h:191
costmap_2d::VoxelLayer::updateBounds
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to up...
Definition: voxel_layer.cpp:116
costmap_2d::VoxelLayer::z_resolution_
double z_resolution_
Definition: voxel_layer.h:173
costmap_2d::VoxelLayer::voxel_pub_
ros::Publisher voxel_pub_
Definition: voxel_layer.h:171
observation_buffer.h
laser_geometry.h
subscriber.h
costmap_2d::VoxelLayer::reset
virtual void reset()
Definition: voxel_layer.cpp:102
costmap_2d::VoxelLayer::matchSize
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
Definition: voxel_layer.cpp:95
costmap_2d::VoxelLayer::setupDynamicReconfigure
virtual void setupDynamicReconfigure(ros::NodeHandle &nh)
Definition: voxel_layer.cpp:67
costmap_2d::VoxelLayer::voxel_dsrv_
dynamic_reconfigure::Server< costmap_2d::VoxelPluginConfig > * voxel_dsrv_
Definition: voxel_layer.h:168
costmap_2d::Costmap2D::size_y_
unsigned int size_y_
Definition: costmap_2d.h:458
costmap_2d::VoxelLayer::unknown_threshold_
unsigned int unknown_threshold_
Definition: voxel_layer.h:174
costmap_2d::VoxelLayer::clearing_endpoints_pub_
ros::Publisher clearing_endpoints_pub_
Definition: voxel_layer.h:175
costmap_2d::Costmap2D::size_x_
unsigned int size_x_
Definition: costmap_2d.h:457
costmap_2d::VoxelLayer::VoxelLayer
VoxelLayer()
Definition: voxel_layer.h:136
point_cloud_conversion.h
costmap_2d::VoxelLayer::clearNonLethal
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
Definition: voxel_layer.cpp:217
costmap_2d::VoxelLayer::publish_voxel_
bool publish_voxel_
Definition: voxel_layer.h:170
costmap_2d::Costmap2D::resolution_
double resolution_
Definition: costmap_2d.h:459
costmap_2d::Observation
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:46
costmap_2d::VoxelLayer::clearing_endpoints_
sensor_msgs::PointCloud clearing_endpoints_
Definition: voxel_layer.h:176
costmap_2d::VoxelLayer::onInitialize
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization.
Definition: voxel_layer.cpp:55
costmap_2d::VoxelLayer::updateOrigin
void updateOrigin(double new_origin_x, double new_origin_y)
Move the origin of the costmap to a new location.... keeping data when it can.
Definition: voxel_layer.cpp:392
costmap_2d::VoxelLayer::size_z_
unsigned int size_z_
Definition: voxel_layer.h:174
costmap_2d::VoxelLayer::mapToWorld3D
void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
Definition: voxel_layer.h:206
costmap_2d::Costmap2D::origin_y_
double origin_y_
Definition: costmap_2d.h:461
costmap_2d::Costmap2D::costmap_
unsigned char * costmap_
Definition: costmap_2d.h:462
costmap_2d::VoxelLayer::mark_threshold_
unsigned int mark_threshold_
Definition: voxel_layer.h:174
costmap_2d
Definition: array_parser.h:37
voxel_grid::VoxelGrid
costmap_2d::VoxelLayer::~VoxelLayer
virtual ~VoxelLayer()
Definition: voxel_layer.cpp:75
ros::NodeHandle


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:17