observation_buffer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, 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 Willow Garage, 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: Eitan Marder-Eppstein
36  *********************************************************************/
38 
42 
43 using namespace std;
44 using namespace tf2;
45 
46 namespace costmap_2d
47 {
48 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
49  double min_obstacle_height, double max_obstacle_height, double obstacle_range,
50  double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame,
51  string sensor_frame, double tf_tolerance) :
52  tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
53  last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
54  min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
55  obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
56 {
57 }
58 
60 {
61 }
62 
63 bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
64 {
65  ros::Time transform_time = ros::Time::now();
66  std::string tf_error;
67 
68  geometry_msgs::TransformStamped transformStamped;
69  if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &tf_error))
70  {
71  ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
72  global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
73  return false;
74  }
75 
76  list<Observation>::iterator obs_it;
77  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
78  {
79  try
80  {
81  Observation& obs = *obs_it;
82 
83  geometry_msgs::PointStamped origin;
84  origin.header.frame_id = global_frame_;
85  origin.header.stamp = transform_time;
86  origin.point = obs.origin_;
87 
88  // we need to transform the origin of the observation to the new global frame
89  tf2_buffer_.transform(origin, origin, new_global_frame);
90  obs.origin_ = origin.point;
91 
92  // we also need to transform the cloud of the observation to the new global frame
93  tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
94  }
95  catch (TransformException& ex)
96  {
97  ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
98  new_global_frame.c_str(), ex.what());
99  return false;
100  }
101  }
102 
103  // now we need to update our global_frame member
104  global_frame_ = new_global_frame;
105  return true;
106 }
107 
108 void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
109 {
110  geometry_msgs::PointStamped global_origin;
111 
112  // create a new observation on the list to be populated
113  observation_list_.push_front(Observation());
114 
115  // check whether the origin frame has been set explicitly or whether we should get it from the cloud
116  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
117 
118  try
119  {
120  // given these observations come from sensors... we'll need to store the origin pt of the sensor
121  geometry_msgs::PointStamped local_origin;
122  local_origin.header.stamp = cloud.header.stamp;
123  local_origin.header.frame_id = origin_frame;
124  local_origin.point.x = 0;
125  local_origin.point.y = 0;
126  local_origin.point.z = 0;
127  tf2_buffer_.transform(local_origin, global_origin, global_frame_);
128  tf2::convert(global_origin.point, observation_list_.front().origin_);
129 
130  // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
131  observation_list_.front().raytrace_range_ = raytrace_range_;
132  observation_list_.front().obstacle_range_ = obstacle_range_;
133 
134  sensor_msgs::PointCloud2 global_frame_cloud;
135 
136  // transform the point cloud
137  tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
138  global_frame_cloud.header.stamp = cloud.header.stamp;
139 
140  // now we need to remove observations from the cloud that are below or above our height thresholds
141  sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
142  observation_cloud.height = global_frame_cloud.height;
143  observation_cloud.width = global_frame_cloud.width;
144  observation_cloud.fields = global_frame_cloud.fields;
145  observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
146  observation_cloud.point_step = global_frame_cloud.point_step;
147  observation_cloud.row_step = global_frame_cloud.row_step;
148  observation_cloud.is_dense = global_frame_cloud.is_dense;
149 
150  unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
151  sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
152  modifier.resize(cloud_size);
153  unsigned int point_count = 0;
154 
155  // copy over the points that are within our height bounds
156  sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
157  std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
158  std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
159  for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
160  {
161  if ((*iter_z) <= max_obstacle_height_
162  && (*iter_z) >= min_obstacle_height_)
163  {
164  std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
165  iter_obs += global_frame_cloud.point_step;
166  ++point_count;
167  }
168  }
169 
170  // resize the cloud for the number of legal points
171  modifier.resize(point_count);
172  observation_cloud.header.stamp = cloud.header.stamp;
173  observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
174  }
175  catch (TransformException& ex)
176  {
177  // if an exception occurs, we need to remove the empty observation from the list
178  observation_list_.pop_front();
179  ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
180  cloud.header.frame_id.c_str(), ex.what());
181  return;
182  }
183 
184  // if the update was successful, we want to update the last updated time
186 
187  // we'll also remove any stale observations from the list
189 }
190 
191 // returns a copy of the observations
192 void ObservationBuffer::getObservations(vector<Observation>& observations)
193 {
194  // first... let's make sure that we don't have any stale observations
196 
197  // now we'll just copy the observations for the caller
198  list<Observation>::iterator obs_it;
199  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
200  {
201  observations.push_back(*obs_it);
202  }
203 }
204 
206 {
207  if (!observation_list_.empty())
208  {
209  list<Observation>::iterator obs_it = observation_list_.begin();
210  // if we're keeping observations for no time... then we'll only keep one observation
212  {
213  observation_list_.erase(++obs_it, observation_list_.end());
214  return;
215  }
216 
217  // otherwise... we'll have to loop through the observations to see which ones are stale
218  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
219  {
220  Observation& obs = *obs_it;
221  // check if the observation is out of date... and if it is, remove it and those that follow from the list
222  if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
223  {
224  observation_list_.erase(obs_it, observation_list_.end());
225  return;
226  }
227  }
228  }
229 }
230 
232 {
234  return true;
235 
236  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
237  if (!current)
238  {
239  ROS_WARN(
240  "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
242  }
243  return current;
244 }
245 
247 {
249 }
250 } // namespace costmap_2d
251 
geometry_msgs::Point origin_
Definition: observation.h:96
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
void getObservations(std::vector< Observation > &observations)
Pushes copies of all current observations onto the end of the vector passed in.
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:46
#define ROS_WARN(...)
bool isCurrent() const
Check if the observation buffer is being update at its expected rate.
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
bool setGlobalFrame(const std::string new_global_frame)
Sets the global frame of an observation buffer. This will transform all the currently cached observat...
void bufferCloud(const sensor_msgs::PointCloud2 &cloud)
Transforms a PointCloud to the global frame and buffers it Note: The burden is on the user to make su...
~ObservationBuffer()
Destructor... cleans up.
void resetLastUpdated()
Reset last updated timestamp.
void purgeStaleObservations()
Removes any stale observations from the buffer list.
sensor_msgs::PointCloud2 * cloud_
Definition: observation.h:97
const ros::Duration expected_update_rate_
std::list< Observation > observation_list_
const ros::Duration observation_keep_time_
static Time now()
void convert(const A &a, B &b)
#define ROS_ERROR(...)


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Jun 22 2022 02:07:03