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 
39 #include <pcl/point_types.h>
40 #include <pcl_ros/transforms.h>
41 #include <pcl/conversions.h>
42 #include <pcl/PCLPointCloud2.h>
43 
45 
46 using namespace std;
47 using namespace tf;
48 
49 namespace costmap_2d
50 {
51 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
52  double min_obstacle_height, double max_obstacle_height, double obstacle_range,
53  double raytrace_range, TransformListener& tf, string global_frame,
54  string sensor_frame, double tf_tolerance) :
55  tf_(tf), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
56  last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
57  min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
58  obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
59 {
60 }
61 
63 {
64 }
65 
66 bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
67 {
68  ros::Time transform_time = ros::Time::now();
69  std::string tf_error;
70 
71  if (!tf_.waitForTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_),
72  ros::Duration(0.01), &tf_error))
73  {
74  ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
75  global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
76  return false;
77  }
78 
79  list<Observation>::iterator obs_it;
80  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
81  {
82  try
83  {
84  Observation& obs = *obs_it;
85 
86  geometry_msgs::PointStamped origin;
87  origin.header.frame_id = global_frame_;
88  origin.header.stamp = transform_time;
89  origin.point = obs.origin_;
90 
91  // we need to transform the origin of the observation to the new global frame
92  tf_.transformPoint(new_global_frame, origin, origin);
93  obs.origin_ = origin.point;
94 
95  // we also need to transform the cloud of the observation to the new global frame
96  pcl_ros::transformPointCloud(new_global_frame, *obs.cloud_, *obs.cloud_, tf_);
97  }
98  catch (TransformException& ex)
99  {
100  ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
101  new_global_frame.c_str(), ex.what());
102  return false;
103  }
104  }
105 
106  // now we need to update our global_frame member
107  global_frame_ = new_global_frame;
108  return true;
109 }
110 
111 void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
112 {
113  try
114  {
115  pcl::PCLPointCloud2 pcl_pc2;
116  pcl_conversions::toPCL(cloud, pcl_pc2);
117  // Actually convert the PointCloud2 message into a type we can reason about
118  pcl::PointCloud < pcl::PointXYZ > pcl_cloud;
119  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
120  bufferCloud(pcl_cloud);
121  }
122  catch (pcl::PCLException& ex)
123  {
124  ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
125  return;
126  }
127 }
128 
129 void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
130 {
131  Stamped < tf::Vector3 > global_origin;
132 
133  // create a new observation on the list to be populated
134  observation_list_.push_front(Observation());
135 
136  // check whether the origin frame has been set explicitly or whether we should get it from the cloud
137  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
138 
139  try
140  {
141  // given these observations come from sensors... we'll need to store the origin pt of the sensor
142  Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
143  pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);
144  tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5));
145  tf_.transformPoint(global_frame_, local_origin, global_origin);
146  observation_list_.front().origin_.x = global_origin.getX();
147  observation_list_.front().origin_.y = global_origin.getY();
148  observation_list_.front().origin_.z = global_origin.getZ();
149 
150  // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
151  observation_list_.front().raytrace_range_ = raytrace_range_;
152  observation_list_.front().obstacle_range_ = obstacle_range_;
153 
154  pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;
155 
156  // transform the point cloud
157  pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
158  global_frame_cloud.header.stamp = cloud.header.stamp;
159 
160  // now we need to remove observations from the cloud that are below or above our height thresholds
161  pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
162  unsigned int cloud_size = global_frame_cloud.points.size();
163  observation_cloud.points.resize(cloud_size);
164  unsigned int point_count = 0;
165 
166  // copy over the points that are within our height bounds
167  for (unsigned int i = 0; i < cloud_size; ++i)
168  {
169  if (global_frame_cloud.points[i].z <= max_obstacle_height_
170  && global_frame_cloud.points[i].z >= min_obstacle_height_)
171  {
172  observation_cloud.points[point_count++] = global_frame_cloud.points[i];
173  }
174  }
175 
176  // resize the cloud for the number of legal points
177  observation_cloud.points.resize(point_count);
178  observation_cloud.header.stamp = cloud.header.stamp;
179  observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
180  }
181  catch (TransformException& ex)
182  {
183  // if an exception occurs, we need to remove the empty observation from the list
184  observation_list_.pop_front();
185  ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
186  cloud.header.frame_id.c_str(), ex.what());
187  return;
188  }
189 
190  // if the update was successful, we want to update the last updated time
192 
193  // we'll also remove any stale observations from the list
195 }
196 
197 // returns a copy of the observations
198 void ObservationBuffer::getObservations(vector<Observation>& observations)
199 {
200  // first... let's make sure that we don't have any stale observations
202 
203  // now we'll just copy the observations for the caller
204  list<Observation>::iterator obs_it;
205  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
206  {
207  observations.push_back(*obs_it);
208  }
209 }
210 
212 {
213  if (!observation_list_.empty())
214  {
215  list<Observation>::iterator obs_it = observation_list_.begin();
216  // if we're keeping observations for no time... then we'll only keep one observation
218  {
219  observation_list_.erase(++obs_it, observation_list_.end());
220  return;
221  }
222 
223  // otherwise... we'll have to loop through the observations to see which ones are stale
224  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
225  {
226  Observation& obs = *obs_it;
227  // check if the observation is out of date... and if it is, remove it and those that follow from the list
228  ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp;
229  if (time_diff > observation_keep_time_)
230  {
231  observation_list_.erase(obs_it, observation_list_.end());
232  return;
233  }
234  }
235  }
236 }
237 
239 {
241  return true;
242 
243  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
244  if (!current)
245  {
246  ROS_WARN(
247  "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
249  }
250  return current;
251 }
252 
254 {
256 }
257 } // namespace costmap_2d
geometry_msgs::Point origin_
Definition: observation.h:97
void getObservations(std::vector< Observation > &observations)
Pushes copies of all current observations onto the end of the vector passed in.
pcl::PointCloud< pcl::PointXYZ > * cloud_
Definition: observation.h:98
tf::TransformListener & tf_
void transformPoint(const std::string &target_frame, const geometry_msgs::PointStamped &stamped_in, geometry_msgs::PointStamped &stamped_out) const
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:47
tf::TransformListener * tf_
#define ROS_WARN(...)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
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.
ros::Time stamp_
std::string frame_id_
const ros::Duration expected_update_rate_
std::list< Observation > observation_list_
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
const ros::Duration observation_keep_time_
static Time now()
bool isCurrent() const
Check if the observation buffer is being update at its expected rate.
#define ROS_ERROR(...)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42