tracker-client.hh
Go to the documentation of this file.
00001 #ifndef VISP_TRACKER_TRACKER_CLIENT_HH
00002 # define VISP_TRACKER_TRACKER_CLIENT_HH
00003 # include <boost/filesystem/fstream.hpp>
00004 # include <boost/filesystem/path.hpp>
00005 # include <boost/thread/recursive_mutex.hpp>
00006 
00007 # include <dynamic_reconfigure/server.h>
00008 
00009 # include <image_proc/advertisement_checker.h>
00010 
00011 # include <image_transport/image_transport.h>
00012 # include <image_transport/subscriber_filter.h>
00013 
00014 # include <message_filters/subscriber.h>
00015 # include <message_filters/sync_policies/approximate_time.h>
00016 # include <message_filters/synchronizer.h>
00017 
00018 # include <sensor_msgs/Image.h>
00019 # include <sensor_msgs/CameraInfo.h>
00020 
00021 # include <resource_retriever/retriever.h>
00022 
00023 # include <visp_tracker/ModelBasedSettingsConfig.h>
00024 # include <visp_tracker/ModelBasedSettingsKltConfig.h>
00025 # include <visp_tracker/ModelBasedSettingsEdgeConfig.h>
00026 # include <visp_tracker/MovingEdgeSites.h>
00027 
00028 # include <visp/vpCameraParameters.h>
00029 # include <visp/vpHomogeneousMatrix.h>
00030 # include <visp/vpImage.h>
00031 # include <visp/vpMbTracker.h>
00032 # include <visp/vpMe.h>
00033 # include <visp/vpKltOpencv.h>
00034 # include <visp/vpPose.h>
00035 
00036 
00037 namespace visp_tracker
00038 {
00039   class TrackerClient
00040   {
00041   public:
00042     typedef vpImage<unsigned char> image_t;
00043     typedef std::vector<vpPoint> points_t;
00044     typedef std::vector<vpImagePoint> imagePoints_t;
00045 
00046     template<class ConfigType>
00047     struct reconfigureSrvStruct{
00048       typedef dynamic_reconfigure::Server<ConfigType> reconfigureSrv_t;
00049     };
00050 
00051     TrackerClient(ros::NodeHandle& nh,
00052                   ros::NodeHandle& privateNh,
00053                   volatile bool& exiting,
00054                   unsigned queueSize = 5u);
00055     
00056     ~TrackerClient();
00057 
00058     void spin();
00059   protected:
00061     void checkInputs();
00062 
00063     void loadModel();
00064 
00065     bool validatePose(const vpHomogeneousMatrix& cMo);
00066     vpHomogeneousMatrix loadInitialPose();
00067     void saveInitialPose(const vpHomogeneousMatrix& cMo);
00068     points_t loadInitializationPoints();
00069 
00070     void init();
00071     void initPoint(unsigned& i,
00072                    points_t& points,
00073                    imagePoints_t& imagePoints,
00074                    ros::Rate& rate,
00075                    vpPose& pose);
00076 
00077 
00078     void waitForImage();
00079 
00080     void sendcMo(const vpHomogeneousMatrix& cMo);
00081 
00082     std::string fetchResource(const std::string&);
00083     bool makeModelFile(boost::filesystem::ofstream& modelStream,
00084                        const std::string& resourcePath,
00085                        std::string& fullModelPath);
00086 
00087   private:
00088     bool exiting ()
00089     {
00090       return exiting_ || !ros::ok();
00091     }
00092 
00093     volatile bool& exiting_;
00094 
00095     unsigned queueSize_;
00096 
00097     ros::NodeHandle& nodeHandle_;
00098     ros::NodeHandle& nodeHandlePrivate_;
00099 
00100     image_transport::ImageTransport imageTransport_;
00101 
00102     image_t image_;
00103 
00104     std::string modelPath_;
00105     std::string modelPathAndExt_;
00106     std::string modelName_;
00107 
00108     std::string cameraPrefix_;
00109     std::string rectifiedImageTopic_;
00110     std::string cameraInfoTopic_;
00111     std::string trackerType_;
00112     double frameSize_;
00113 
00114     boost::filesystem::path bModelPath_;
00115     boost::filesystem::path bInitPath_;
00116 
00117     image_transport::CameraSubscriber cameraSubscriber_;
00118 
00119     boost::recursive_mutex mutex_;
00120     reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t *reconfigureSrv_;
00121     reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t *reconfigureKltSrv_;
00122     reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t *reconfigureEdgeSrv_;
00123 
00124     std_msgs::Header header_;
00125     sensor_msgs::CameraInfoConstPtr info_;
00126 
00127     vpMe movingEdge_;
00128     vpKltOpencv kltTracker_;
00129     vpCameraParameters cameraParameters_;
00130     vpMbTracker *tracker_;
00131 
00132     bool startFromSavedPose_;
00133     bool confirmInit_;
00134 
00136     image_proc::AdvertisementChecker checkInputs_;
00137 
00138     resource_retriever::Retriever resourceRetriever_;
00139   };
00140 } // end of namespace visp_tracker.
00141 
00142 #endif //! VISP_TRACKER_TRACKER_CLIENT_HH


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