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
00040 #ifndef CART_LOCAL_PLANNER_SBPL_SUBSCRIBER_H
00041 #define CART_LOCAL_PLANNER_SBPL_SUBSCRIBER_H
00042
00043 #include <geometry_msgs/PoseStamped.h>
00044 #include <ros/ros.h>
00045 #include <boost/optional.hpp>
00046 #include <boost/thread.hpp>
00047 #include <boost/circular_buffer.hpp>
00048
00049 namespace cart_local_planner
00050 {
00051
00052 template <class SBPLPlan>
00053 class SBPLSubscriber
00054 {
00055 public:
00056 typedef std::vector<geometry_msgs::PoseStamped> MBPlan;
00057
00058 SBPLSubscriber (const ros::NodeHandle& nh, const std::string& topic, const double timeout=0.01,
00059 const unsigned buffer_size=10) :
00060 nh_(nh), sub_(nh_.subscribe(topic, buffer_size, &SBPLSubscriber<SBPLPlan>::planCB, this)),
00061 plan_buffer_(buffer_size), timeout_(timeout)
00062 {
00063 }
00064
00065 boost::optional<SBPLPlan> lookupPlan (const MBPlan& mb_plan) const
00066 {
00067 const double num_waits = 10.0;
00068 ros::Rate r(num_waits/timeout_);
00069 boost::optional<SBPLPlan> plan;
00070 for (unsigned i=0; i<num_waits; i++) {
00071 r.sleep();
00072 boost::mutex::scoped_lock l(mutex_);
00073 typename boost::circular_buffer<SBPLPlan>::const_reverse_iterator iter =
00074 find(plan_buffer_.rbegin(), plan_buffer_.rend(), mb_plan);
00075 if (iter!=plan_buffer_.rend()) {
00076 plan = *iter;
00077 return plan;
00078 }
00079 }
00080
00081 return plan;
00082 }
00083
00084 private:
00085
00086 mutable boost::mutex mutex_;
00087 ros::NodeHandle nh_;
00088 ros::Subscriber sub_;
00089 boost::circular_buffer<SBPLPlan> plan_buffer_;
00090 const double timeout_;
00091
00092 void planCB (const SBPLPlan& sbpl_plan)
00093 {
00094 boost::mutex::scoped_lock l(mutex_);
00095 plan_buffer_.push_back(sbpl_plan);
00096 }
00097
00098 };
00099
00100 }
00101
00102 #endif // include guard