Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef COSTMAP_2D_VOXEL_LAYER_H_
00039 #define COSTMAP_2D_VOXEL_LAYER_H_
00040
00041 #include <ros/ros.h>
00042 #include <costmap_2d/layer.h>
00043 #include <costmap_2d/layered_costmap.h>
00044 #include <costmap_2d/observation_buffer.h>
00045 #include <costmap_2d/VoxelGrid.h>
00046 #include <nav_msgs/OccupancyGrid.h>
00047 #include <sensor_msgs/LaserScan.h>
00048 #include <laser_geometry/laser_geometry.h>
00049 #include <sensor_msgs/PointCloud.h>
00050 #include <sensor_msgs/PointCloud2.h>
00051 #include <sensor_msgs/point_cloud_conversion.h>
00052 #include <tf/message_filter.h>
00053 #include <message_filters/subscriber.h>
00054 #include <dynamic_reconfigure/server.h>
00055 #include <costmap_2d/VoxelPluginConfig.h>
00056 #include <costmap_2d/obstacle_layer.h>
00057 #include <voxel_grid/voxel_grid.h>
00058
00059 namespace costmap_2d
00060 {
00061
00062 class VoxelLayer : public ObstacleLayer
00063 {
00064 public:
00065 VoxelLayer() :
00066 voxel_grid_(0, 0, 0)
00067 {
00068 costmap_ = NULL;
00069 }
00070
00071 virtual ~VoxelLayer();
00072
00073 virtual void onInitialize();
00074 virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00075 double* max_x, double* max_y);
00076
00077 void updateOrigin(double new_origin_x, double new_origin_y);
00078 bool isDiscretized()
00079 {
00080 return true;
00081 }
00082 virtual void matchSize();
00083 virtual void reset();
00084
00085
00086 protected:
00087 virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
00088
00089 virtual void resetMaps();
00090
00091 private:
00092 void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level);
00093 void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
00094 virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
00095 double* max_x, double* max_y);
00096
00097 dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
00098
00099 bool publish_voxel_;
00100 ros::Publisher voxel_pub_;
00101 voxel_grid::VoxelGrid voxel_grid_;
00102 double z_resolution_, origin_z_;
00103 unsigned int unknown_threshold_, mark_threshold_, size_z_;
00104 ros::Publisher clearing_endpoints_pub_;
00105 sensor_msgs::PointCloud clearing_endpoints_;
00106
00107 inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
00108 {
00109 if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
00110 return false;
00111 mx = ((wx - origin_x_) / resolution_);
00112 my = ((wy - origin_y_) / resolution_);
00113 mz = ((wz - origin_z_) / z_resolution_);
00114 if (mx < size_x_ && my < size_y_ && mz < size_z_)
00115 return true;
00116
00117 return false;
00118 }
00119
00120 inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz)
00121 {
00122 if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
00123 return false;
00124
00125 mx = (int)((wx - origin_x_) / resolution_);
00126 my = (int)((wy - origin_y_) / resolution_);
00127 mz = (int)((wz - origin_z_) / z_resolution_);
00128
00129 if (mx < size_x_ && my < size_y_ && mz < size_z_)
00130 return true;
00131
00132 return false;
00133 }
00134
00135 inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz)
00136 {
00137
00138 wx = origin_x_ + (mx + 0.5) * resolution_;
00139 wy = origin_y_ + (my + 0.5) * resolution_;
00140 wz = origin_z_ + (mz + 0.5) * z_resolution_;
00141 }
00142
00143 inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
00144 {
00145 return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
00146 }
00147 };
00148
00149 }
00150
00151 #endif // COSTMAP_2D_VOXEL_LAYER_H_
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15