observation_buffer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Eitan Marder-Eppstein
00036  *********************************************************************/
00037 #include <costmap_2d/observation_buffer.h>
00038 
00039 #include <pcl/point_types.h>
00040 #include <pcl_ros/transforms.h>
00041 #include <pcl/conversions.h>
00042 #include <pcl/PCLPointCloud2.h>
00043 
00044 #include <pcl_conversions/pcl_conversions.h>
00045 
00046 using namespace std;
00047 using namespace tf;
00048 
00049 namespace costmap_2d
00050 {
00051 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
00052                                      double min_obstacle_height, double max_obstacle_height, double obstacle_range,
00053                                      double raytrace_range, TransformListener& tf, string global_frame,
00054                                      string sensor_frame, double tf_tolerance) :
00055     tf_(tf), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
00056     last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
00057     min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
00058     obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
00059 {
00060 }
00061 
00062 ObservationBuffer::~ObservationBuffer()
00063 {
00064 }
00065 
00066 bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
00067 {
00068   ros::Time transform_time = ros::Time::now();
00069   std::string tf_error;
00070 
00071   if (!tf_.waitForTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_),
00072                             ros::Duration(0.01), &tf_error))
00073   {
00074     ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
00075               global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
00076     return false;
00077   }
00078 
00079   list<Observation>::iterator obs_it;
00080   for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
00081   {
00082     try
00083     {
00084       Observation& obs = *obs_it;
00085 
00086       geometry_msgs::PointStamped origin;
00087       origin.header.frame_id = global_frame_;
00088       origin.header.stamp = transform_time;
00089       origin.point = obs.origin_;
00090 
00091       // we need to transform the origin of the observation to the new global frame
00092       tf_.transformPoint(new_global_frame, origin, origin);
00093       obs.origin_ = origin.point;
00094 
00095       // we also need to transform the cloud of the observation to the new global frame
00096       pcl_ros::transformPointCloud(new_global_frame, *obs.cloud_, *obs.cloud_, tf_);
00097     }
00098     catch (TransformException& ex)
00099     {
00100       ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
00101                 new_global_frame.c_str(), ex.what());
00102       return false;
00103     }
00104   }
00105 
00106   // now we need to update our global_frame member
00107   global_frame_ = new_global_frame;
00108   return true;
00109 }
00110 
00111 void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
00112 {
00113   try
00114   {
00115     pcl::PCLPointCloud2 pcl_pc2;
00116     pcl_conversions::toPCL(cloud, pcl_pc2);
00117     // Actually convert the PointCloud2 message into a type we can reason about
00118     pcl::PointCloud < pcl::PointXYZ > pcl_cloud;
00119     pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
00120     bufferCloud(pcl_cloud);
00121   }
00122   catch (pcl::PCLException& ex)
00123   {
00124     ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
00125     return;
00126   }
00127 }
00128 
00129 void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
00130 {
00131   Stamped < tf::Vector3 > global_origin;
00132 
00133   // create a new observation on the list to be populated
00134   observation_list_.push_front(Observation());
00135 
00136   // check whether the origin frame has been set explicitly or whether we should get it from the cloud
00137   string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
00138 
00139   try
00140   {
00141     // given these observations come from sensors... we'll need to store the origin pt of the sensor
00142     Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
00143                             pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);
00144     tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5));
00145     tf_.transformPoint(global_frame_, local_origin, global_origin);
00146     observation_list_.front().origin_.x = global_origin.getX();
00147     observation_list_.front().origin_.y = global_origin.getY();
00148     observation_list_.front().origin_.z = global_origin.getZ();
00149 
00150     // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
00151     observation_list_.front().raytrace_range_ = raytrace_range_;
00152     observation_list_.front().obstacle_range_ = obstacle_range_;
00153 
00154     pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;
00155 
00156     // transform the point cloud
00157     pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
00158     global_frame_cloud.header.stamp = cloud.header.stamp;
00159 
00160     // now we need to remove observations from the cloud that are below or above our height thresholds
00161     pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
00162     unsigned int cloud_size = global_frame_cloud.points.size();
00163     observation_cloud.points.resize(cloud_size);
00164     unsigned int point_count = 0;
00165 
00166     // copy over the points that are within our height bounds
00167     for (unsigned int i = 0; i < cloud_size; ++i)
00168     {
00169       if (global_frame_cloud.points[i].z <= max_obstacle_height_
00170           && global_frame_cloud.points[i].z >= min_obstacle_height_)
00171       {
00172         observation_cloud.points[point_count++] = global_frame_cloud.points[i];
00173       }
00174     }
00175 
00176     // resize the cloud for the number of legal points
00177     observation_cloud.points.resize(point_count);
00178     observation_cloud.header.stamp = cloud.header.stamp;
00179     observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
00180   }
00181   catch (TransformException& ex)
00182   {
00183     // if an exception occurs, we need to remove the empty observation from the list
00184     observation_list_.pop_front();
00185     ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
00186               cloud.header.frame_id.c_str(), ex.what());
00187     return;
00188   }
00189 
00190   // if the update was successful, we want to update the last updated time
00191   last_updated_ = ros::Time::now();
00192 
00193   // we'll also remove any stale observations from the list
00194   purgeStaleObservations();
00195 }
00196 
00197 // returns a copy of the observations
00198 void ObservationBuffer::getObservations(vector<Observation>& observations)
00199 {
00200   // first... let's make sure that we don't have any stale observations
00201   purgeStaleObservations();
00202 
00203   // now we'll just copy the observations for the caller
00204   list<Observation>::iterator obs_it;
00205   for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
00206   {
00207     observations.push_back(*obs_it);
00208   }
00209 }
00210 
00211 void ObservationBuffer::purgeStaleObservations()
00212 {
00213   if (!observation_list_.empty())
00214   {
00215     list<Observation>::iterator obs_it = observation_list_.begin();
00216     // if we're keeping observations for no time... then we'll only keep one observation
00217     if (observation_keep_time_ == ros::Duration(0.0))
00218     {
00219       observation_list_.erase(++obs_it, observation_list_.end());
00220       return;
00221     }
00222 
00223     // otherwise... we'll have to loop through the observations to see which ones are stale
00224     for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
00225     {
00226       Observation& obs = *obs_it;
00227       // check if the observation is out of date... and if it is, remove it and those that follow from the list
00228       ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp;
00229       if ((last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp) > observation_keep_time_)
00230       {
00231         observation_list_.erase(obs_it, observation_list_.end());
00232         return;
00233       }
00234     }
00235   }
00236 }
00237 
00238 bool ObservationBuffer::isCurrent() const
00239 {
00240   if (expected_update_rate_ == ros::Duration(0.0))
00241     return true;
00242 
00243   bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
00244   if (!current)
00245   {
00246     ROS_WARN(
00247         "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
00248         topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
00249   }
00250   return current;
00251 }
00252 
00253 void ObservationBuffer::resetLastUpdated()
00254 {
00255   last_updated_ = ros::Time::now();
00256 }
00257 }  // namespace costmap_2d
00258 


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21