00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <costmap_2d/observation_buffer.h>
00038
00039 #include <pcl/point_types.h>
00040 #include <pcl_ros/transforms.h>
00041 #include <pcl/ros/conversions.h>
00042
00043 using namespace std;
00044 using namespace tf;
00045
00046 namespace costmap_2d {
00047 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
00048 double min_obstacle_height, double max_obstacle_height, double obstacle_range, double raytrace_range,
00049 TransformListener& tf, string global_frame, string sensor_frame, double tf_tolerance) : tf_(tf),
00050 observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate), last_updated_(ros::Time::now()),
00051 global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name), min_obstacle_height_(min_obstacle_height),
00052 max_obstacle_height_(max_obstacle_height), obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
00053 {
00054 }
00055
00056 ObservationBuffer::~ObservationBuffer(){}
00057
00058 bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame){
00059 ros::Time transform_time = ros::Time::now();
00060 std::string tf_error;
00061
00062 if(!tf_.waitForTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), ros::Duration(0.01), &tf_error)){
00063 ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
00064 return false;
00065 }
00066
00067 list<Observation>::iterator obs_it;
00068 for(obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
00069 try{
00070 Observation& obs = *obs_it;
00071
00072 geometry_msgs::PointStamped origin;
00073 origin.header.frame_id = global_frame_;
00074 origin.header.stamp = transform_time;
00075 origin.point = obs.origin_;
00076
00077
00078 tf_.transformPoint(new_global_frame, origin, origin);
00079 obs.origin_ = origin.point;
00080
00081
00082 pcl_ros::transformPointCloud(new_global_frame, obs.cloud_, obs.cloud_, tf_);
00083
00084 }
00085 catch(TransformException& ex){
00086 ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
00087 new_global_frame.c_str(), ex.what());
00088 return false;
00089 }
00090 }
00091
00092
00093 global_frame_ = new_global_frame;
00094 return true;
00095 }
00096
00097 void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud){
00098 try {
00099
00100 pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
00101 pcl::fromROSMsg(cloud, pcl_cloud);
00102 bufferCloud(pcl_cloud);
00103 }
00104 catch(pcl::PCLException& ex){
00105 ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
00106 return;
00107 }
00108 }
00109
00110 void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud){
00111 Stamped<tf::Vector3> global_origin;
00112
00113
00114 observation_list_.push_front(Observation());
00115
00116
00117 string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
00118
00119 try{
00120
00121 Stamped<tf::Vector3> local_origin(tf::Vector3(0, 0, 0), cloud.header.stamp, origin_frame);
00122 tf_.transformPoint(global_frame_, local_origin, global_origin);
00123 observation_list_.front().origin_.x = global_origin.getX();
00124 observation_list_.front().origin_.y = global_origin.getY();
00125 observation_list_.front().origin_.z = global_origin.getZ();
00126
00127
00128 observation_list_.front().raytrace_range_ = raytrace_range_;
00129 observation_list_.front().obstacle_range_ = obstacle_range_;
00130
00131 pcl::PointCloud<pcl::PointXYZ> global_frame_cloud;
00132
00133
00134 pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
00135 global_frame_cloud.header.stamp = cloud.header.stamp;
00136
00137
00138 pcl::PointCloud<pcl::PointXYZ>& observation_cloud = observation_list_.front().cloud_;
00139 unsigned int cloud_size = global_frame_cloud.points.size();
00140 observation_cloud.points.resize(cloud_size);
00141 unsigned int point_count = 0;
00142
00143
00144 for(unsigned int i = 0; i < cloud_size; ++i){
00145 if(global_frame_cloud.points[i].z <= max_obstacle_height_ && global_frame_cloud.points[i].z >= min_obstacle_height_){
00146 observation_cloud.points[point_count++] = global_frame_cloud.points[i];
00147 }
00148 }
00149
00150
00151 observation_cloud.points.resize(point_count);
00152 observation_cloud.header.stamp = cloud.header.stamp;
00153 observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
00154 }
00155 catch(TransformException& ex){
00156
00157 observation_list_.pop_front();
00158 ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
00159 cloud.header.frame_id.c_str(), ex.what());
00160 return;
00161 }
00162
00163
00164 last_updated_ = ros::Time::now();
00165
00166
00167 purgeStaleObservations();
00168
00169 }
00170
00171
00172 void ObservationBuffer::getObservations(vector<Observation>& observations){
00173
00174 purgeStaleObservations();
00175
00176
00177 list<Observation>::iterator obs_it;
00178 for(obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
00179 observations.push_back(*obs_it);
00180 }
00181
00182 }
00183
00184 void ObservationBuffer::purgeStaleObservations(){
00185 if(!observation_list_.empty()){
00186 list<Observation>::iterator obs_it = observation_list_.begin();
00187
00188 if(observation_keep_time_ == ros::Duration(0.0)){
00189 observation_list_.erase(++obs_it, observation_list_.end());
00190 return;
00191 }
00192
00193
00194 for(obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
00195 Observation& obs = *obs_it;
00196
00197 ros::Duration time_diff = last_updated_ - obs.cloud_.header.stamp;
00198 if((last_updated_ - obs.cloud_.header.stamp) > observation_keep_time_){
00199 observation_list_.erase(obs_it, observation_list_.end());
00200 return;
00201 }
00202 }
00203 }
00204 }
00205
00206 bool ObservationBuffer::isCurrent() const {
00207 if(expected_update_rate_ == ros::Duration(0.0))
00208 return true;
00209
00210 bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
00211 if(!current){
00212 ROS_WARN("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", topic_name_.c_str(),
00213 (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
00214 }
00215 return current;
00216 }
00217
00218 void ObservationBuffer::resetLastUpdated ()
00219 {
00220 last_updated_ = ros::Time::now();
00221 }
00222 };