38 #ifndef COSTMAP_2D_VOXEL_LAYER_H_ 39 #define COSTMAP_2D_VOXEL_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> 54 #include <dynamic_reconfigure/server.h> 55 #include <costmap_2d/VoxelPluginConfig.h> 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);
77 void updateOrigin(
double new_origin_x,
double new_origin_y);
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);
95 double* max_x,
double* max_y);
97 dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *
voxel_dsrv_;
107 inline bool worldToMap3DFloat(
double wx,
double wy,
double wz,
double& mx,
double& my,
double& mz)
120 inline bool worldToMap3D(
double wx,
double wy,
double wz,
unsigned int& mx,
unsigned int& my,
unsigned int& mz)
135 inline void mapToWorld3D(
unsigned int mx,
unsigned int my,
unsigned int mz,
double& wx,
double& wy,
double& wz)
143 inline double dist(
double x0,
double y0,
double z0,
double x1,
double y1,
double z1)
145 return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
151 #endif // COSTMAP_2D_VOXEL_LAYER_H_ unsigned int unknown_threshold_
sensor_msgs::PointCloud clearing_endpoints_
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_
ros::Publisher clearing_endpoints_pub_
unsigned int mark_threshold_
voxel_grid::VoxelGrid voxel_grid_
Stores an observation in terms of a point cloud and the origin of the source.
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
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)
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization...
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)
dynamic_reconfigure::Server< costmap_2d::VoxelPluginConfig > * voxel_dsrv_
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.
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)
void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)