41 #ifndef VOLUME_GRID_LAYER_H_ 42 #define VOLUME_GRID_LAYER_H_ 46 #include <spatio_temporal_voxel_layer/SpatioTemporalVoxelLayerConfig.h> 50 #include <dynamic_reconfigure/server.h> 57 #include <openvdb/openvdb.h> 64 #include <sensor_msgs/LaserScan.h> 65 #include <sensor_msgs/PointCloud2.h> 67 #include <geometry_msgs/Point.h> 68 #include <spatio_temporal_voxel_layer/SaveGrid.h> 69 #include <std_srvs/SetBool.h> 94 virtual void updateBounds(
double robot_x,
double robot_y,
double robot_yaw, \
95 double* min_x,
double* min_y,
double* max_x,
double* max_y);
97 int max_i,
int max_j);
103 virtual void reset(
void);
114 void UpdateROSCostmap(
double* min_x,
double* min_y,
double* max_x,
double* max_y);
115 bool updateFootprint(
double robot_x,
double robot_y,
double robot_yaw, \
116 double* min_x,
double* min_y,
double* max_x,
double* max_y);
120 bool SaveGridCallback(spatio_temporal_voxel_layer::SaveGrid::Request& req, \
121 spatio_temporal_voxel_layer::SaveGrid::Response& resp);
141 std_srvs::SetBool::Response & response, \
volume_grid::GlobalDecayModel _decay_model
virtual void onInitialize(void)
void PointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr &message, const boost::shared_ptr< buffer::MeasurementBuffer > &buffer)
std::vector< geometry_msgs::Point > _transformed_footprint
std::vector< boost::shared_ptr< buffer::MeasurementBuffer > >::iterator observation_buffers_iter
void UpdateROSCostmap(double *min_x, double *min_y, double *max_x, double *max_y)
bool _update_footprint_enabled
bool updateFootprint(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
bool RemoveStaticObservations(void)
std::string _global_frame
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
virtual void activate(void)
ros::ServiceServer _grid_saver
ros::Publisher _voxel_pub
dynamic_reconfigure::Server< dynamicReconfigureType > dynamicReconfigureServerType
virtual void deactivate(void)
void LaserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr &raw_message, const boost::shared_ptr< buffer::MeasurementBuffer > &buffer)
std::vector< boost::shared_ptr< buffer::MeasurementBuffer > > _observation_buffers
void ObservationsResetAfterReading() const
std::vector< boost::shared_ptr< buffer::MeasurementBuffer > > _marking_buffers
std::vector< boost::shared_ptr< tf::MessageFilterBase > > _observation_notifiers
bool GetClearingObservations(std::vector< observation::MeasurementReading > &marking_observations) const
std::vector< boost::shared_ptr< message_filters::SubscriberBase > >::iterator observation_subscribers_iter
ros::Duration _map_save_duration
ros::Time _last_map_save_time
virtual ~SpatioTemporalVoxelLayer(void)
boost::recursive_mutex _voxel_grid_lock
volume_grid::SpatioTemporalVoxelGrid * _voxel_grid
virtual void matchSize(void)
std::vector< boost::shared_ptr< message_filters::SubscriberBase > > _observation_subscribers
std::vector< boost::shared_ptr< buffer::MeasurementBuffer > > _clearing_buffers
dynamicReconfigureServerType * _dynamic_reconfigure_server
spatio_temporal_voxel_layer::SpatioTemporalVoxelLayerConfig dynamicReconfigureType
bool GetMarkingObservations(std::vector< observation::MeasurementReading > &marking_observations) const
laser_geometry::LaserProjection _laser_projector
bool AddStaticObservations(const observation::MeasurementReading &obs)
SpatioTemporalVoxelLayer(void)
std::vector< ros::ServiceServer > _buffer_enabler_servers
bool SaveGridCallback(spatio_temporal_voxel_layer::SaveGrid::Request &req, spatio_temporal_voxel_layer::SaveGrid::Response &resp)
bool BufferEnablerCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response, boost::shared_ptr< buffer::MeasurementBuffer > &buffer, boost::shared_ptr< message_filters::SubscriberBase > &subcriber)
std::vector< observation::MeasurementReading > _static_observations
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
void DynamicReconfigureCallback(dynamicReconfigureType &config, uint32_t level)
void LaserScanCallback(const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< buffer::MeasurementBuffer > &buffer)