visp_tracker: visp_tracker: tracker.cpp Source File

00001 #include <boost/bind.hpp>
00002 #include <boost/make_shared.hpp>
00003 #include <boost/shared_ptr.hpp>
00004 #include <boost/thread.hpp>
00005 
00006 #include <nodelet/nodelet.h>
00007 #include <pluginlib/class_list_macros.h>
00008 
00009 #include "tracker.hh"
00010 
00011 namespace visp_tracker
00012 {
00013   class TrackerNodelet : public nodelet::Nodelet
00014   {
00015   public:
00016     TrackerNodelet ()
00017       : nodelet::Nodelet (),
00018         exiting_ (false),
00019         tracker_ (),
00020         thread_ ()
00021     {}
00022 
00023     ~TrackerNodelet ()
00024     {
00025       exiting_ = true;
00026       if (thread_)
00027         if (!thread_->timed_join (boost::posix_time::seconds (2)))
00028           NODELET_WARN ("failed to join thread but continuing anyway");
00029       thread_.reset ();
00030       tracker_.reset ();
00031     }
00032 
00033     void spin ()
00034     {
00035       if (exiting_)
00036         return;
00037       tracker_ = boost::shared_ptr<visp_tracker::Tracker>
00038         (new visp_tracker::Tracker (getMTNodeHandle (),
00039                                     getMTPrivateNodeHandle (),
00040                                     exiting_, 5u));
00041       while (ros::ok () && !exiting_)
00042         tracker_->spin ();
00043     }
00044 
00045     virtual void onInit ()
00046     {
00047       NODELET_DEBUG ("Initializing nodelet...");
00048       exiting_ = false;
00049       thread_ = boost::make_shared<boost::thread>
00050         (boost::bind (&TrackerNodelet::spin, this));
00051     }
00052 
00053   private:
00054     volatile bool exiting_;
00055     boost::shared_ptr<visp_tracker::Tracker> tracker_;
00056     boost::shared_ptr<boost::thread> thread_;
00057   };
00058 
00059 } // end of namespace visp_tracker.
00060 
00061 PLUGINLIB_DECLARE_CLASS(visp_tracker, Tracker,
00062                         visp_tracker::TrackerNodelet, nodelet::Nodelet);