tracker-viewer.hh
Go to the documentation of this file.
00001 #ifndef VISP_TRACKER_TRACKER_VIEWER_HH
00002 # define VISP_TRACKER_TRACKER_VIEWER_HH
00003 # include <boost/filesystem/path.hpp>
00004 # include <boost/optional.hpp>
00005 
00006 # include <geometry_msgs/PoseWithCovarianceStamped.h>
00007 
00008 # include <image_proc/advertisement_checker.h>
00009 
00010 # include <image_transport/image_transport.h>
00011 # include <image_transport/subscriber_filter.h>
00012 
00013 # include <message_filters/subscriber.h>
00014 # include <message_filters/sync_policies/approximate_time.h>
00015 # include <message_filters/synchronizer.h>
00016 
00017 # include <sensor_msgs/Image.h>
00018 # include <sensor_msgs/CameraInfo.h>
00019 
00020 # include <visp_tracker/Init.h>
00021 # include <visp_tracker/MovingEdgeSites.h>
00022 # include <visp_tracker/KltPoints.h>
00023 
00024 # include <visp/vpCameraParameters.h>
00025 # include <visp/vpImage.h>
00026 # include <visp/vpMbEdgeTracker.h>
00027 
00028 namespace visp_tracker
00029 {
00031   class TrackerViewer
00032   {
00033   public:
00035     typedef vpImage<unsigned char> image_t;
00036 
00037 
00038     typedef boost::function<bool (visp_tracker::Init::Request&,
00039           visp_tracker::Init::Response& res)>
00040     initCallback_t;
00041 
00042     typedef boost::function<bool (visp_tracker::Init::Request&,
00043           visp_tracker::Init::Response& res)>
00044     reconfigureCallback_t;
00045 
00053     typedef message_filters::sync_policies::ApproximateTime<
00054       sensor_msgs::Image, sensor_msgs::CameraInfo,
00055       geometry_msgs::PoseWithCovarianceStamped,
00056       visp_tracker::MovingEdgeSites,
00057       visp_tracker::KltPoints
00058       > syncPolicy_t;
00059 
00061     TrackerViewer(ros::NodeHandle& nh,
00062                   ros::NodeHandle& privateNh,
00063                   volatile bool& exiting,
00064                   unsigned queueSize = 5u);
00065 
00068     void spin();
00069   protected:
00071     void initializeTracker();
00072 
00074     void loadCommonParameters();
00075 
00077     void checkInputs();
00078 
00080     void waitForImage();
00081 
00082     bool initCallback(visp_tracker::Init::Request& req,
00083           visp_tracker::Init::Response& res);
00084 
00085     bool reconfigureCallback(visp_tracker::Init::Request& req,
00086           visp_tracker::Init::Response& res);
00087 
00089     void
00090     callback
00091     (const sensor_msgs::ImageConstPtr& imageConst,
00092      const sensor_msgs::CameraInfoConstPtr& infoConst,
00093      const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult,
00094      const visp_tracker::MovingEdgeSites::ConstPtr& sitesConst,
00095      const visp_tracker::KltPoints::ConstPtr& kltConst);
00096 
00097     void timerCallback();
00098 
00100     void displayMovingEdgeSites();
00102     void displayKltPoints();
00103 
00104   private:
00105     bool exiting ()
00106     {
00107       return exiting_ || !ros::ok();
00108     }
00109 
00110     volatile bool& exiting_;
00111 
00113     unsigned queueSize_;
00114 
00115     ros::NodeHandle& nodeHandle_;
00116     ros::NodeHandle& nodeHandlePrivate_;
00117 
00119     image_transport::ImageTransport imageTransport_;
00120 
00121     double frameSize_;
00122 
00125 
00127     std::string rectifiedImageTopic_;
00129     std::string cameraInfoTopic_;
00130 
00132 
00134     ros::ServiceServer initService_;
00135 
00137     ros::ServiceServer reconfigureService_;
00138 
00140     std::string trackerName_;
00141 
00143     boost::filesystem::path modelPath_;
00144 
00146     image_proc::AdvertisementChecker checkInputs_;
00147 
00149     vpMbEdgeTracker tracker_;
00151     vpCameraParameters cameraParameters_;
00153     image_t image_;
00154 
00156     sensor_msgs::CameraInfoConstPtr info_;
00158     boost::optional<vpHomogeneousMatrix> cMo_;
00160     visp_tracker::MovingEdgeSites::ConstPtr sites_;
00162     visp_tracker::KltPoints::ConstPtr klt_;
00163 
00167     image_transport::SubscriberFilter imageSubscriber_;
00169     message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSubscriber_;
00171     message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>
00172     trackingResultSubscriber_;
00174     message_filters::Subscriber<visp_tracker::MovingEdgeSites>
00175     movingEdgeSitesSubscriber_;
00177     message_filters::Subscriber<visp_tracker::KltPoints>
00178     kltPointsSubscriber_;
00179 
00181     message_filters::Synchronizer<syncPolicy_t> synchronizer_;
00183 
00186     ros::WallTimer timer_;
00187     unsigned countAll_;
00188     unsigned countImages_;
00189     unsigned countCameraInfo_;
00190     unsigned countTrackingResult_;
00191     unsigned countMovingEdgeSites_;
00192     unsigned countKltPoints_;
00194   };
00195 } // end of namespace visp_tracker
00196 
00197 #endif //! VISP_TRACKER_TRACKER_VIEWER_HH


visp_tracker
Author(s): Thomas Moulard
autogenerated on Sun Feb 19 2017 03:28:46