queue.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2017, Alexander Stumpf, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef VIGIR_FOOTSTEP_PLANNING_THREADING_QUEUE_H__
00030 #define VIGIR_FOOTSTEP_PLANNING_THREADING_QUEUE_H__
00031 
00032 #include <ros/ros.h>
00033 
00034 #include <queue>
00035 #include <boost/thread/mutex.hpp>
00036 #include <boost/thread/condition_variable.hpp>
00037 
00038 
00039 
00040 namespace vigir_footstep_planning
00041 {
00042 namespace threading
00043 {
00044 template <class T>
00045 class Queue
00046 {
00047 public:
00048   Queue()
00049     : job_counter(0)
00050   {
00051   }
00052 
00053   virtual ~Queue()
00054   {
00055   }
00056 
00057   void clear()
00058   {
00059     boost::mutex::scoped_lock lock(queued_jobs_mutex);
00060     queued_jobs = typename std::queue<boost::shared_ptr<T> >();
00061     job_counter = 0;
00062     jobs_finished_condition.notify_all();
00063   }
00064 
00065   void queueJob(boost::shared_ptr<T>& job)
00066   {
00067     { // scope needed so mutex gets unlocked when other threads are notified
00068       boost::mutex::scoped_lock lock(queued_jobs_mutex);
00069       queued_jobs.push(job);
00070     }
00071     queued_jobs_condition.notify_one();
00072   }
00073 
00074   void queueJobs(std::list<boost::shared_ptr<T> >& jobs)
00075   {
00076     { // scope needed so mutex gets unlocked when other threads are notified
00077       boost::mutex::scoped_lock lock(queued_jobs_mutex);
00078       for (typename std::list<boost::shared_ptr<T> >::iterator itr = jobs.begin(); itr != jobs.end(); itr++)
00079         queued_jobs.push(*itr);
00080     }
00081     queued_jobs_condition.notify_all();
00082   }
00083 
00084   boost::shared_ptr<T>& waitAndDequeueJob()
00085   {
00086     boost::mutex::scoped_lock lock(queued_jobs_mutex);
00087 
00088     if (queued_jobs.empty())
00089     {
00090       ROS_DEBUG("[Queue] Waiting for Job getting queued...");
00091       queued_jobs_condition.wait(lock);
00092     }
00093 
00094     ROS_DEBUG("[Queue] Pop Job.");
00095 
00096     boost::shared_ptr<T>& front = queued_jobs.front();
00097     queued_jobs.pop();
00098     job_counter++;
00099 
00100     return front;
00101   }
00102 
00103   void waitAndDequeueJobs(std::vector<boost::shared_ptr<T> >& jobs, unsigned int n)
00104   {
00105     boost::mutex::scoped_lock lock(queued_jobs_mutex);
00106 
00107     if (queued_jobs.empty())
00108     {
00109       ROS_DEBUG("[Queue] Waiting for Job getting queued...");
00110       queued_jobs_condition.wait(lock);
00111     }
00112 
00113     ROS_DEBUG("[Queue] Pop %u Job(s).", n);
00114     n = std::min(n, static_cast<unsigned int>(queued_jobs.size()));
00115     jobs.resize(n);
00116 
00117     for (unsigned int i = 0; i < n; i++)
00118     {
00119       jobs[i] = queued_jobs.front();
00120       queued_jobs.pop();
00121       job_counter++;
00122     }
00123   }
00124 
00125   void justFinishedJobs(unsigned int n)
00126   {
00127     boost::mutex::scoped_lock lock(queued_jobs_mutex);
00128 
00129     if (job_counter > n)
00130       job_counter -= n;
00131     else
00132       job_counter = 0;
00133 
00134     if (queued_jobs.empty() && job_counter == 0)
00135     {
00136       ROS_DEBUG("[Queue] Notify for finished job.");
00137       jobs_finished_condition.notify_all();
00138     }
00139   }
00140 
00141   bool hasOpenJobs() const
00142   {
00143     boost::mutex::scoped_lock lock(queued_jobs_mutex);
00144     return !queued_jobs.empty() || job_counter != 0;
00145   }
00146 
00147   void waitUntilJobsProcessed() const
00148   {
00149     boost::mutex::scoped_lock lock(queued_jobs_mutex);
00150 
00151     if (!queued_jobs.empty())
00152     {
00153       ROS_DEBUG("[Queue] Waiting for Jobs getting finished...");
00154       jobs_finished_condition.wait(lock);
00155       ROS_DEBUG("[Queue] Waiting for Jobs getting finished...Done!");
00156     }
00157   }
00158 
00159 protected:
00160   std::queue<boost::shared_ptr<T> > queued_jobs;
00161   unsigned int job_counter;
00162 
00163   mutable boost::mutex queued_jobs_mutex;
00164 
00165   mutable boost::condition_variable queued_jobs_condition;
00166   mutable boost::condition_variable jobs_finished_condition;
00167 };
00168 }
00169 }
00170 
00171 #endif


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56