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), last_updated_(
00056         ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name), min_obstacle_height_(
00057         min_obstacle_height), max_obstacle_height_(max_obstacle_height), obstacle_range_(obstacle_range), raytrace_range_(
00058         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     }
00099     catch (TransformException& ex)
00100     {
00101       ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
00102                 new_global_frame.c_str(), ex.what());
00103       return false;
00104     }
00105   }
00106 
00107   //now we need to update our global_frame member
00108   global_frame_ = new_global_frame;
00109   return true;
00110 }
00111 
00112 void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
00113 {
00114   try
00115   {
00116     pcl::PCLPointCloud2 pcl_pc2;
00117     pcl_conversions::toPCL(cloud, pcl_pc2);
00118     // Actually convert the PointCloud2 message into a type we can reason about
00119     pcl::PointCloud < pcl::PointXYZ > pcl_cloud;
00120     pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
00121     bufferCloud (pcl_cloud);
00122   }
00123   catch (pcl::PCLException& ex)
00124   {
00125     ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
00126     return;
00127   }
00128 }
00129 
00130 void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
00131 {
00132   Stamped < tf::Vector3 > global_origin;
00133 
00134   //create a new observation on the list to be populated
00135   observation_list_.push_front(Observation());
00136 
00137   //check whether the origin frame has been set explicitly or whether we should get it from the cloud
00138   string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
00139 
00140   try
00141   {
00142     //given these observations come from sensors... we'll need to store the origin pt of the sensor
00143     Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0), 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 the costmap will see
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 
00198 //returns a copy of the observations
00199 void ObservationBuffer::getObservations(vector<Observation>& observations)
00200 {
00201   //first... let's make sure that we don't have any stale observations
00202   purgeStaleObservations();
00203 
00204   //now we'll just copy the observations for the caller
00205   list<Observation>::iterator obs_it;
00206   for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
00207   {
00208     observations.push_back(*obs_it);
00209   }
00210 
00211 }
00212 
00213 void ObservationBuffer::purgeStaleObservations()
00214 {
00215   if (!observation_list_.empty())
00216   {
00217     list<Observation>::iterator obs_it = observation_list_.begin();
00218     //if we're keeping observations for no time... then we'll only keep one observation
00219     if (observation_keep_time_ == ros::Duration(0.0))
00220     {
00221       observation_list_.erase(++obs_it, observation_list_.end());
00222       return;
00223     }
00224 
00225     //otherwise... we'll have to loop through the observations to see which ones are stale
00226     for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
00227     {
00228       Observation& obs = *obs_it;
00229       //check if the observation is out of date... and if it is, remove it and those that follow from the list
00230       ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp;
00231       if ((last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp) > observation_keep_time_)
00232       {
00233         observation_list_.erase(obs_it, observation_list_.end());
00234         return;
00235       }
00236     }
00237   }
00238 }
00239 
00240 bool ObservationBuffer::isCurrent() const
00241 {
00242   if (expected_update_rate_ == ros::Duration(0.0))
00243     return true;
00244 
00245   bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
00246   if (!current)
00247   {
00248     ROS_WARN(
00249         "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
00250         topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
00251   }
00252   return current;
00253 }
00254 
00255 void ObservationBuffer::resetLastUpdated()
00256 {
00257   last_updated_ = ros::Time::now();
00258 }
00259 }
00260 


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:55