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>
52 #include <tf/message_filter.h>
54 #include <dynamic_reconfigure/server.h>
55 #include <costmap_2d/VoxelPluginConfig.h>
57 #include <voxel_grid/voxel_grid.h>
58 
59 namespace costmap_2d
60 {
61 
62 class VoxelLayer : public ObstacleLayer
63 {
64 public:
66  voxel_grid_(0, 0, 0)
67  {
68  costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
69  }
70 
71  virtual ~VoxelLayer();
72 
73  virtual void onInitialize();
74  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
75  double* max_x, double* max_y);
76 
77  void updateOrigin(double new_origin_x, double new_origin_y);
79  {
80  return true;
81  }
82  virtual void matchSize();
83  virtual void reset();
84 
85 
86 protected:
87  virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
88 
89  virtual void resetMaps();
90 
91 private:
92  void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level);
93  void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
94  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
95  double* max_x, double* max_y);
96 
97  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
98 
105  sensor_msgs::PointCloud clearing_endpoints_;
106 
107  inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
108  {
109  if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
110  return false;
111  mx = ((wx - origin_x_) / resolution_);
112  my = ((wy - origin_y_) / resolution_);
113  mz = ((wz - origin_z_) / z_resolution_);
114  if (mx < size_x_ && my < size_y_ && mz < size_z_)
115  return true;
116 
117  return false;
118  }
119 
120  inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz)
121  {
122  if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
123  return false;
124 
125  mx = (int)((wx - origin_x_) / resolution_);
126  my = (int)((wy - origin_y_) / resolution_);
127  mz = (int)((wz - origin_z_) / z_resolution_);
128 
129  if (mx < size_x_ && my < size_y_ && mz < size_z_)
130  return true;
131 
132  return false;
133  }
134 
135  inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz)
136  {
137  // returns the center point of the cell
138  wx = origin_x_ + (mx + 0.5) * resolution_;
139  wy = origin_y_ + (my + 0.5) * resolution_;
140  wz = origin_z_ + (mz + 0.5) * z_resolution_;
141  }
142 
143  inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
144  {
145  return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
146  }
147 };
148 
149 } // namespace costmap_2d
150 
151 #endif // COSTMAP_2D_VOXEL_LAYER_H_
unsigned int unknown_threshold_
Definition: voxel_layer.h:103
sensor_msgs::PointCloud clearing_endpoints_
Definition: voxel_layer.h:105
virtual void resetMaps()
Resets the costmap and static_map to be unknown space.
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
ros::Publisher voxel_pub_
Definition: voxel_layer.h:100
unsigned int size_y_
Definition: costmap_2d.h:422
ros::Publisher clearing_endpoints_pub_
Definition: voxel_layer.h:104
unsigned int mark_threshold_
Definition: voxel_layer.h:103
voxel_grid::VoxelGrid voxel_grid_
Definition: voxel_layer.h:101
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:47
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
Definition: voxel_layer.cpp:95
unsigned int size_x_
Definition: costmap_2d.h:421
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
Definition: voxel_layer.h:143
void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
Definition: voxel_layer.h:135
virtual void reset()
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.
virtual void setupDynamicReconfigure(ros::NodeHandle &nh)
Definition: voxel_layer.cpp:67
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization...
Definition: voxel_layer.cpp:55
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...
bool worldToMap3D(double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
Definition: voxel_layer.h:120
dynamic_reconfigure::Server< costmap_2d::VoxelPluginConfig > * voxel_dsrv_
Definition: voxel_layer.h:97
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.
unsigned int size_z_
Definition: voxel_layer.h:103
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
bool worldToMap3DFloat(double wx, double wy, double wz, double &mx, double &my, double &mz)
Definition: voxel_layer.h:107
unsigned char * costmap_
Definition: costmap_2d.h:426
void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
Definition: voxel_layer.cpp:81


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