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/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
00092 tf_.transformPoint(new_global_frame, origin, origin);
00093 obs.origin_ = origin.point;
00094
00095
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
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
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
00134 observation_list_.push_front(Observation());
00135
00136
00137 string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
00138
00139 try
00140 {
00141
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
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
00157 pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
00158 global_frame_cloud.header.stamp = cloud.header.stamp;
00159
00160
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
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
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
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
00191 last_updated_ = ros::Time::now();
00192
00193
00194 purgeStaleObservations();
00195 }
00196
00197
00198 void ObservationBuffer::getObservations(vector<Observation>& observations)
00199 {
00200
00201 purgeStaleObservations();
00202
00203
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
00217 if (observation_keep_time_ == ros::Duration(0.0))
00218 {
00219 observation_list_.erase(++obs_it, observation_list_.end());
00220 return;
00221 }
00222
00223
00224 for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
00225 {
00226 Observation& obs = *obs_it;
00227
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 }
00258