measurement_buffer.cpp
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  *********************************************************************/
37 
41 
42 namespace buffer
43 {
44 
45 /*****************************************************************************/
46 MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \
47  const double& observation_keep_time, \
48  const double& expected_update_rate, \
49  const double& min_obstacle_height, \
50  const double& max_obstacle_height, \
51  const double& obstacle_range, \
53  const std::string& global_frame, \
54  const std::string& sensor_frame, \
55  const double& tf_tolerance, \
56  const double& min_d, \
57  const double& max_d, \
58  const double& vFOV, \
59  const double& vFOVPadding, \
60  const double& hFOV, \
61  const double& decay_acceleration, \
62  const bool& marking, \
63  const bool& clearing, \
64  const double& voxel_size, \
65  const Filters& filter, \
66  const int& voxel_min_points, \
67  const bool& enabled, \
68  const bool& clear_buffer_after_reading, \
69  const ModelType& model_type) :
70 /*****************************************************************************/
71  _buffer(tf), _observation_keep_time(observation_keep_time),
72  _expected_update_rate(expected_update_rate),_last_updated(ros::Time::now()),
73  _global_frame(global_frame), _sensor_frame(sensor_frame),
74  _topic_name(topic_name), _min_obstacle_height(min_obstacle_height),
75  _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range),
76  _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d),
77  _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding),
78  _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration),
79  _marking(marking), _clearing(clearing), _voxel_size(voxel_size),
80  _filter(filter), _voxel_min_points(voxel_min_points),
81  _enabled(enabled), _clear_buffer_after_reading(clear_buffer_after_reading),
82  _model_type(model_type)
83 {
84 }
85 
86 /*****************************************************************************/
88 /*****************************************************************************/
89 {
90 }
91 
92 /*****************************************************************************/
93 void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud)
94 /*****************************************************************************/
95 {
96  // add a new measurement to be populated
98 
99  const std::string origin_frame = \
100  _sensor_frame == "" ? cloud.header.frame_id : _sensor_frame;
101 
102  try
103  {
104  // transform into global frame
105  geometry_msgs::PoseStamped local_pose, global_pose;
106  local_pose.pose.position.x=0;
107  local_pose.pose.position.y=0;
108  local_pose.pose.position.z=0;
109  local_pose.pose.orientation.x=0;
110  local_pose.pose.orientation.y=0;
111  local_pose.pose.orientation.z=0;
112  local_pose.pose.orientation.w=1;
113  local_pose.header.stamp = cloud.header.stamp;
114  local_pose.header.frame_id = origin_frame;
115 
116  _buffer.canTransform(_global_frame, local_pose.header.frame_id , \
117  local_pose.header.stamp, ros::Duration(0.5));
118  _buffer.transform(local_pose, global_pose, _global_frame);
119 
120  _observation_list.front()._origin.x = global_pose.pose.position.x;
121  _observation_list.front()._origin.y = global_pose.pose.position.y;
122  _observation_list.front()._origin.z = global_pose.pose.position.z;
123 
124  _observation_list.front()._orientation = global_pose.pose.orientation;
125  _observation_list.front()._obstacle_range_in_m = _obstacle_range;
126  _observation_list.front()._min_z_in_m = _min_z;
127  _observation_list.front()._max_z_in_m = _max_z;
128  _observation_list.front()._vertical_fov_in_rad = _vertical_fov;
129  _observation_list.front()._vertical_fov_padding_in_m = _vertical_fov_padding;
130  _observation_list.front()._horizontal_fov_in_rad = _horizontal_fov;
131  _observation_list.front()._decay_acceleration = _decay_acceleration;
132  _observation_list.front()._clearing = _clearing;
133  _observation_list.front()._marking = _marking;
134  _observation_list.front()._model_type = _model_type;
135 
136  if (_clearing && !_marking)
137  {
138  // no need to buffer points
139  return;
140  }
141 
142  // transform the cloud in the global frame
143  point_cloud_ptr cld_global(new sensor_msgs::PointCloud2());
144  geometry_msgs::TransformStamped tf_stamped = _buffer.lookupTransform( \
145  _global_frame, cloud.header.frame_id, cloud.header.stamp);
146  tf2::doTransform (cloud, *cld_global, tf_stamped);
147 
148  pcl::PCLPointCloud2::Ptr cloud_pcl (new pcl::PCLPointCloud2 ());
149  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
150 
151  // remove points that are below or above our height restrictions, and
152  // in the same time, remove NaNs and if user wants to use it, combine with a
153  if (_filter == Filters::VOXEL)
154  {
155  pcl_conversions::toPCL(*cld_global, *cloud_pcl);
156  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
157  sor.setInputCloud (cloud_pcl);
158  sor.setFilterFieldName("z");
159  sor.setFilterLimits(_min_obstacle_height,_max_obstacle_height);
160  sor.setDownsampleAllData(false);
161  sor.setLeafSize ((float)_voxel_size,
162  (float)_voxel_size,
163  (float)_voxel_size);
164  sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
165  sor.filter(*cloud_filtered);
166  pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
167  }
168  else if (_filter == Filters::PASSTHROUGH)
169  {
170  pcl_conversions::toPCL(*cld_global, *cloud_pcl);
171  pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
172  pass_through_filter.setInputCloud(cloud_pcl);
173  pass_through_filter.setKeepOrganized(false);
174  pass_through_filter.setFilterFieldName("z");
175  pass_through_filter.setFilterLimits( \
177  pass_through_filter.filter(*cloud_filtered);
178  pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
179  }
180 
181  _observation_list.front()._cloud = cld_global;
182  }
183  catch (tf::TransformException& ex)
184  {
185  // if fails, remove the empty observation
186  _observation_list.pop_front();
187  ROS_ERROR( \
188  "TF Exception for sensor frame: %s, cloud frame: %s, %s", \
189  _sensor_frame.c_str(), cloud.header.frame_id.c_str(), ex.what());
190  return;
191  }
192 
195 }
196 
197 /*****************************************************************************/
199  std::vector<observation::MeasurementReading>& observations)
200 /*****************************************************************************/
201 {
203 
204  for (readings_iter it = _observation_list.begin(); \
205  it != _observation_list.end(); ++it)
206  {
207  observations.push_back(*it);
208  }
209 }
210 
211 /*****************************************************************************/
213 /*****************************************************************************/
214 {
215  if (_observation_list.empty())
216  {
217  return;
218  }
219 
220  readings_iter it = _observation_list.begin();
222  {
223  _observation_list.erase(++it, _observation_list.end());
224  return;
225  }
226 
227  for (it = _observation_list.begin(); it != _observation_list.end(); ++it)
228  {
230  const ros::Duration time_diff = _last_updated - obs._cloud->header.stamp;
231 
232  if (time_diff > _observation_keep_time)
233  {
234  _observation_list.erase(it, _observation_list.end());
235  return;
236  }
237  }
238 }
239 
240 /*****************************************************************************/
242 /*****************************************************************************/
243 {
244  _observation_list.clear();
245 }
246 
247 /*****************************************************************************/
249 /*****************************************************************************/
250 {
252 }
253 
254 /*****************************************************************************/
256 /*****************************************************************************/
257 {
258  if (_expected_update_rate == ros::Duration(0.0))
259  {
260  return true;
261  }
262 
263  const ros::Duration update_time = ros::Time::now() - _last_updated;
264  const bool current = update_time.toSec() <= _expected_update_rate.toSec();
265  if (!current)
266  {
267  ROS_WARN_THROTTLE(10.,
268  "%s buffer updated in %.2fs, it should be updated every %.2fs.",
269  _topic_name.c_str(), update_time.toSec(), _expected_update_rate.toSec());
270  }
271  return current;
272 }
273 
274 /*****************************************************************************/
275 bool MeasurementBuffer::IsEnabled(void) const
276 /*****************************************************************************/
277 {
278  return _enabled;
279 }
280 
281 /*****************************************************************************/
282 void MeasurementBuffer::SetEnabled(const bool& enabled)
283 /*****************************************************************************/
284 {
285  _enabled = enabled;
286 }
287 
288 /*****************************************************************************/
290 /*****************************************************************************/
291 {
292  _last_updated = ros::Time::now();
293 }
294 
295 /*****************************************************************************/
296 void MeasurementBuffer::Lock(void)
297 /*****************************************************************************/
298 {
299  _lock.lock();
300 }
301 
302 /*****************************************************************************/
303 void MeasurementBuffer::Unlock(void)
304 /*****************************************************************************/
305 {
306  _lock.unlock();
307 }
308 
309 } // namespace buffer
310 
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
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(period,...)
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
pcl_conversions::toPCL
void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
tf2_sensor_msgs.h
buffer::MeasurementBuffer::_decay_acceleration
double _decay_acceleration
Definition: measurement_buffer.hpp:180
buffer::MeasurementBuffer::_observation_list
std::list< observation::MeasurementReading > _observation_list
Definition: measurement_buffer.hpp:177
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
tf2_ros::Buffer::canTransform
virtual bool canTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout, std::string *errstr=NULL) const
pcl_conversions::fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
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
observation::MeasurementReading::_cloud
sensor_msgs::PointCloud2::Ptr _cloud
Definition: measurement_reading.h:115
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
observation::MeasurementReading
Definition: measurement_reading.h:57
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::_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
measurement_buffer.hpp
buffer::Filters::VOXEL
@ VOXEL
buffer::MeasurementBuffer::SetEnabled
void SetEnabled(const bool &enabled)
Definition: measurement_buffer.cpp:317
buffer::MeasurementBuffer::_obstacle_range
double _obstacle_range
Definition: measurement_buffer.hpp:178
tf2_ros::BufferInterface::transform
B & transform(const A &in, B &out, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
buffer::MeasurementBuffer::_voxel_size
double _voxel_size
Definition: measurement_buffer.hpp:180
buffer::MeasurementBuffer::Lock
void Lock(void)
Definition: measurement_buffer.cpp:331
buffer::MeasurementBuffer::_max_obstacle_height
double _max_obstacle_height
Definition: measurement_buffer.hpp:178
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
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
ROS_ERROR
#define ROS_ERROR(...)
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
tf2_geometry_msgs.h
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
DurationBase< Duration >::toSec
double toSec() const
buffer::MeasurementBuffer::_observation_keep_time
const ros::Duration _observation_keep_time
Definition: measurement_buffer.hpp:173
tf2::TransformException
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
tf2_ros::Buffer::lookupTransform
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout) const
buffer::MeasurementBuffer::ResetAllMeasurements
void ResetAllMeasurements(void)
Definition: measurement_buffer.cpp:276
buffer::MeasurementBuffer::_last_updated
ros::Time _last_updated
Definition: measurement_buffer.hpp:175
ros::Time::now
static Time now()


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