measurement_buffer.hpp
Go to the documentation of this file.
1 /********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2018, Simbe Robotics, 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 Simbe Robotics, 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: Steve Macenski (steven.macenski@simberobotics.com)
36  * Purpose: buffer incoming measurements for grid
37  *********************************************************************/
38 
39 #ifndef MEASUREMENT_BUFFER_H_
40 #define MEASUREMENT_BUFFER_H_
41 
42 // measurement structs
44 // PCL
45 #include <pcl_ros/transforms.h>
46 #include <pcl/filters/voxel_grid.h>
48 #include <pcl/filters/passthrough.h>
49 // STL
50 #include <vector>
51 #include <list>
52 #include <string>
53 // ROS
54 #include <ros/ros.h>
55 #include <ros/time.h>
56 // TF
57 #include <tf2_ros/buffer.h>
59 // msgs
60 #include <sensor_msgs/PointCloud2.h>
61 #include <geometry_msgs/Quaternion.h>
62 #include <geometry_msgs/PoseStamped.h>
63 #include <geometry_msgs/TransformStamped.h>
64 // Mutex
65 #include <boost/thread.hpp>
66 
67 namespace buffer
68 {
69 
70 enum class Filters
71 {
72  NONE = 0,
73  VOXEL = 1,
74  PASSTHROUGH = 2
75 };
76 
77 // conveniences for line lengths
78 typedef std::list<observation::MeasurementReading>::iterator readings_iter;
79 typedef sensor_msgs::PointCloud2::Ptr point_cloud_ptr;
80 
81 // Measurement buffer
83 {
84 public:
85  MeasurementBuffer(const std::string& topic_name, \
86  const double& observation_keep_time, \
87  const double& expected_update_rate, \
88  const double& min_obstacle_height, \
89  const double& max_obstacle_height, \
90  const double& obstacle_range, \
92  const std::string& global_frame, \
93  const std::string& sensor_frame, \
94  const double& tf_tolerance, \
95  const double& min_d, \
96  const double& max_d, \
97  const double& vFOV, \
98  const double& vFOVPadding, \
99  const double& hFOV, \
100  const double& decay_acceleration, \
101  const bool& marking, \
102  const bool& clearing, \
103  const double& voxel_size, \
104  const Filters& filter, \
105  const int& voxel_min_points, \
106  const bool& enabled, \
107  const bool& clear_buffer_after_reading, \
108  const ModelType& model_type);
109 
110  ~MeasurementBuffer(void);
111 
112  // Buffers for different types of pointclouds
113  void BufferROSCloud(const sensor_msgs::PointCloud2& cloud);
114 
115  // Get measurements from the buffer
116  void GetReadings(std::vector<observation::MeasurementReading>& observations);
117 
118  // enabler setter getter
119  bool IsEnabled(void) const;
120  void SetEnabled(const bool& enabled);
121 
122  // State knoweldge if sensors are operating as expected
123  bool UpdatedAtExpectedRate(void) const;
124  void ResetLastUpdatedTime(void);
125  void ResetAllMeasurements(void);
126  bool ClearAfterReading(void);
127 
128  // Public mutex locks
129  void Lock(void);
130  void Unlock(void);
131 
132 private:
133  // Removing old observations from buffer
134  void RemoveStaleObservations(void);
135 
138  boost::recursive_mutex _lock;
141  std::list<observation::MeasurementReading> _observation_list;
147  int _voxel_min_points;
149 };
150 
151 } // namespace buffer
152 
153 #endif // MEASUREMENT_BUFFER_H_
buffer::MeasurementBuffer::_model_type
ModelType _model_type
Definition: measurement_buffer.hpp:184
buffer::MeasurementBuffer::_clearing
bool _clearing
Definition: measurement_buffer.hpp:181
buffer::MeasurementBuffer::RemoveStaleObservations
void RemoveStaleObservations(void)
Definition: measurement_buffer.cpp:247
buffer::Filters::PASSTHROUGH
@ PASSTHROUGH
buffer::MeasurementBuffer::_min_z
double _min_z
Definition: measurement_buffer.hpp:179
ros.h
time.h
buffer::MeasurementBuffer::Unlock
void Unlock(void)
Definition: measurement_buffer.cpp:338
buffer::MeasurementBuffer::_clear_buffer_after_reading
bool _clear_buffer_after_reading
Definition: measurement_buffer.hpp:181
buffer
Definition: measurement_buffer.hpp:67
buffer.h
buffer::MeasurementBuffer::_decay_acceleration
double _decay_acceleration
Definition: measurement_buffer.hpp:180
buffer::MeasurementBuffer::_topic_name
std::string _topic_name
Definition: measurement_buffer.hpp:176
buffer::MeasurementBuffer::_observation_list
std::list< observation::MeasurementReading > _observation_list
Definition: measurement_buffer.hpp:177
transforms.h
buffer::MeasurementBuffer::_voxel_min_points
int _voxel_min_points
Definition: measurement_buffer.hpp:183
buffer::MeasurementBuffer::~MeasurementBuffer
~MeasurementBuffer(void)
Definition: measurement_buffer.cpp:122
buffer::MeasurementBuffer::GetReadings
void GetReadings(std::vector< observation::MeasurementReading > &observations)
Definition: measurement_buffer.cpp:233
buffer::MeasurementBuffer::_sensor_frame
std::string _sensor_frame
Definition: measurement_buffer.hpp:176
buffer::MeasurementBuffer::_vertical_fov
double _vertical_fov
Definition: measurement_buffer.hpp:179
buffer::MeasurementBuffer::BufferROSCloud
void BufferROSCloud(const sensor_msgs::PointCloud2 &cloud)
Definition: measurement_buffer.cpp:128
buffer::readings_iter
std::list< observation::MeasurementReading >::iterator readings_iter
Definition: measurement_buffer.hpp:114
buffer::MeasurementBuffer::_buffer
tf2_ros::Buffer & _buffer
Definition: measurement_buffer.hpp:172
subscriber.h
buffer::MeasurementBuffer::MeasurementBuffer
MeasurementBuffer(const std::string &topic_name, const double &observation_keep_time, const double &expected_update_rate, const double &min_obstacle_height, const double &max_obstacle_height, const double &obstacle_range, tf2_ros::Buffer &tf, const std::string &global_frame, const std::string &sensor_frame, const double &tf_tolerance, const double &min_d, const double &max_d, const double &vFOV, const double &vFOVPadding, const double &hFOV, const double &decay_acceleration, const bool &marking, const bool &clearing, const double &voxel_size, const Filters &filter, const int &voxel_min_points, const bool &enabled, const bool &clear_buffer_after_reading, const ModelType &model_type)
Definition: measurement_buffer.cpp:81
buffer::MeasurementBuffer::_max_z
double _max_z
Definition: measurement_buffer.hpp:179
buffer::MeasurementBuffer
Definition: measurement_buffer.hpp:118
buffer::MeasurementBuffer::_horizontal_fov
double _horizontal_fov
Definition: measurement_buffer.hpp:179
buffer::MeasurementBuffer::_min_obstacle_height
double _min_obstacle_height
Definition: measurement_buffer.hpp:178
buffer::MeasurementBuffer::IsEnabled
bool IsEnabled(void) const
Definition: measurement_buffer.cpp:310
tf2_ros::Buffer
buffer::MeasurementBuffer::_global_frame
std::string _global_frame
Definition: measurement_buffer.hpp:176
buffer::Filters::VOXEL
@ VOXEL
buffer::MeasurementBuffer::_tf_tolerance
double _tf_tolerance
Definition: measurement_buffer.hpp:178
buffer::Filters::NONE
@ NONE
buffer::MeasurementBuffer::SetEnabled
void SetEnabled(const bool &enabled)
Definition: measurement_buffer.cpp:317
buffer::MeasurementBuffer::_enabled
bool _enabled
Definition: measurement_buffer.hpp:181
buffer::MeasurementBuffer::_obstacle_range
double _obstacle_range
Definition: measurement_buffer.hpp:178
buffer::MeasurementBuffer::_voxel_size
double _voxel_size
Definition: measurement_buffer.hpp:180
buffer::MeasurementBuffer::Lock
void Lock(void)
Definition: measurement_buffer.cpp:331
ros::Time
buffer::MeasurementBuffer::_max_obstacle_height
double _max_obstacle_height
Definition: measurement_buffer.hpp:178
buffer::MeasurementBuffer::_marking
bool _marking
Definition: measurement_buffer.hpp:181
buffer::MeasurementBuffer::_vertical_fov_padding
double _vertical_fov_padding
Definition: measurement_buffer.hpp:179
buffer::MeasurementBuffer::UpdatedAtExpectedRate
bool UpdatedAtExpectedRate(void) const
Definition: measurement_buffer.cpp:290
buffer::Filters
Filters
Definition: measurement_buffer.hpp:106
buffer::MeasurementBuffer::_filter
Filters _filter
Definition: measurement_buffer.hpp:182
ModelType
ModelType
Definition: measurement_reading.h:47
buffer::MeasurementBuffer::_expected_update_rate
const ros::Duration _expected_update_rate
Definition: measurement_buffer.hpp:173
tf
buffer::MeasurementBuffer::ResetLastUpdatedTime
void ResetLastUpdatedTime(void)
Definition: measurement_buffer.cpp:324
buffer::point_cloud_ptr
sensor_msgs::PointCloud2::Ptr point_cloud_ptr
Definition: measurement_buffer.hpp:115
buffer::MeasurementBuffer::_observation_keep_time
const ros::Duration _observation_keep_time
Definition: measurement_buffer.hpp:173
ros::Duration
buffer::MeasurementBuffer::_lock
boost::recursive_mutex _lock
Definition: measurement_buffer.hpp:174
buffer::MeasurementBuffer::ClearAfterReading
bool ClearAfterReading(void)
Definition: measurement_buffer.cpp:283
buffer::MeasurementBuffer::ResetAllMeasurements
void ResetAllMeasurements(void)
Definition: measurement_buffer.cpp:276
measurement_reading.h
buffer::MeasurementBuffer::_last_updated
ros::Time _last_updated
Definition: measurement_buffer.hpp:175
pcl_conversions.h


spatio_temporal_voxel_layer
Author(s):
autogenerated on Wed Aug 16 2023 02:10:02