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
00039 #include <graph_slam/localization_buffer.h>
00040 #include <graph_slam/exception.h>
00041 #include <graph_mapping_utils/geometry.h>
00042 #include <graph_mapping_utils/msg.h>
00043
00044 namespace graph_slam
00045 {
00046
00047 namespace msg=graph_mapping_msgs;
00048 namespace util=graph_mapping_utils;
00049 namespace gm=geometry_msgs;
00050
00051 using std::string;
00052
00053 typedef msg::LocalizationDistribution::ConstPtr LocPtr;
00054 typedef boost::mutex::scoped_lock Lock;
00055 typedef boost::circular_buffer<LocPtr> Buf;
00056
00057
00058 LocalizationBuffer::LocalizationBuffer (const unsigned size, const string& fixed_frame, const string& base_frame, TfPtr tf,
00059 const double max_extrapolation) :
00060 fixed_frame_(fixed_frame), base_frame_(base_frame), max_extrapolation_(max_extrapolation), tf_(tf),
00061 buffer_(size), loc_sub_(nh_.subscribe("graph_localization", 5, &LocalizationBuffer::localizationCallback, this))
00062 {}
00063
00064 void LocalizationBuffer::localizationCallback (LocPtr m)
00065 {
00066 Lock lock(mutex_);
00067
00068
00069 if (!buffer_.empty()) {
00070 LocPtr last_loc = buffer_[buffer_.size()-1];
00071 const ros::Time& last_stamp = last_loc->stamp;
00072 if (last_stamp >= m->stamp) {
00073 ROS_WARN_STREAM ("Not adding localization with stamp " << m->stamp <<
00074 " to buffer because last stamp was " << last_stamp);
00075 return;
00076 }
00077 }
00078
00079 buffer_.push_back(m);
00080 }
00081
00082 struct LocalizationOrdering
00083 {
00084 bool operator() (const ros::Time& t, const LocPtr& l)
00085 {
00086 return (t < l->stamp);
00087 }
00088 };
00089
00090 gm::PoseStamped LocalizationBuffer::localizationAt (const ros::Time& t) const
00091 {
00092 ros::Duration d(0.0);
00093 return localizationAt(t, d);
00094 }
00095
00096 gm::PoseStamped LocalizationBuffer::localizationAt (const ros::Time& t, const ros::Duration& timeout) const
00097 {
00098 LocPtr nearest;
00099 tf::StampedTransform trans;
00100 {
00101 Lock lock(mutex_);
00102 if (buffer_.empty())
00103 throw LocalizationExtrapolationException(t);
00104
00105 const Buf::const_iterator pos = upper_bound(buffer_.begin(), buffer_.end(), t, LocalizationOrdering());
00106 nearest = (pos == buffer_.begin()) ? *pos : *(pos-1);
00107 const double gap = fabs(nearest->stamp.toSec() - t.toSec());
00108 if (gap > max_extrapolation_)
00109 throw LocalizationExtrapolationException (t, nearest->stamp);
00110
00111
00112 tf_->waitForTransform(base_frame_, fixed_frame_, nearest->stamp, timeout);
00113 tf_->waitForTransform(base_frame_, fixed_frame_, t, timeout);
00114 tf_->lookupTransform(base_frame_, nearest->stamp, base_frame_, t, fixed_frame_, trans);
00115 }
00116
00117
00118 gm::PoseStamped l;
00119 l.header.frame_id = nearest->samples[0].header.frame_id;
00120 l.pose = util::absolutePose(nearest->samples[0].pose, trans);
00121 return l;
00122 }
00123
00124 bool LocalizationBuffer::hasLocalization (const ros::Time& t) const
00125 {
00126 Lock l(mutex_);
00127 bool has_loc = false;
00128 if (!buffer_.empty()) {
00129 Buf::const_iterator pos = upper_bound(buffer_.begin(), buffer_.end(), t, LocalizationOrdering());
00130 LocPtr nearest = (pos == buffer_.begin()) ? *pos : *(pos-1);
00131 has_loc = fabs(nearest->stamp.toSec() - t.toSec()) < max_extrapolation_;
00132 ROS_DEBUG_STREAM_COND_NAMED (!has_loc, "localization", "Nearest localization was " << nearest->stamp);
00133 }
00134 ROS_DEBUG_STREAM_COND_NAMED (!has_loc, "localization", "No localization found at " << t
00135 << "; buffer size is " << buffer_.size());
00136 return has_loc;
00137 }
00138
00139
00140 boost::optional<gm::PoseStamped> LocalizationBuffer::lastLocalization () const
00141 {
00142 Lock lock(mutex_);
00143 boost::optional<gm::PoseStamped> l;
00144 if (!buffer_.empty()) {
00145 LocPtr dist = buffer_[buffer_.size()-1];
00146 ROS_ASSERT_MSG (dist->samples.size()==1, "Assumption of deterministic localization violated");
00147 l = dist->samples[0];
00148 }
00149 return l;
00150 }
00151
00152 }