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/MovingEdgeConfig.h> 00024 # include <visp_tracker/MovingEdgeSites.h> 00025 00026 # include <visp/vpCameraParameters.h> 00027 # include <visp/vpHomogeneousMatrix.h> 00028 # include <visp/vpImage.h> 00029 # include <visp/vpMbEdgeTracker.h> 00030 # include <visp/vpPose.h> 00031 00032 00033 namespace visp_tracker 00034 { 00035 class TrackerClient 00036 { 00037 public: 00038 typedef vpImage<unsigned char> image_t; 00039 typedef std::vector<vpPoint> points_t; 00040 typedef std::vector<vpImagePoint> imagePoints_t; 00041 00042 typedef dynamic_reconfigure::Server<visp_tracker::MovingEdgeConfig> 00043 reconfigureSrv_t; 00044 00045 typedef dynamic_reconfigure::Server 00046 <visp_tracker::MovingEdgeConfig>::CallbackType 00047 reconfigureCallback_t; 00048 00049 00050 TrackerClient(ros::NodeHandle& nh, 00051 ros::NodeHandle& privateNh, 00052 volatile bool& exiting, 00053 unsigned queueSize = 5u); 00054 00055 void spin(); 00056 protected: 00058 void checkInputs(); 00059 00060 void loadModel(); 00061 00062 bool validatePose(const vpHomogeneousMatrix& cMo); 00063 vpHomogeneousMatrix loadInitialPose(); 00064 void saveInitialPose(const vpHomogeneousMatrix& cMo); 00065 points_t loadInitializationPoints(); 00066 00067 void init(); 00068 void initPoint(unsigned& i, 00069 points_t& points, 00070 imagePoints_t& imagePoints, 00071 ros::Rate& rate, 00072 vpPose& pose); 00073 00074 00075 void waitForImage(); 00076 00077 void sendcMo(const vpHomogeneousMatrix& cMo); 00078 00079 std::string fetchResource(const std::string&); 00080 bool makeModelFile(boost::filesystem::ofstream& modelStream, 00081 const std::string& resourcePath, 00082 std::string& fullModelPath); 00083 00084 private: 00085 bool exiting () 00086 { 00087 return exiting_ || !ros::ok(); 00088 } 00089 00090 volatile bool& exiting_; 00091 00092 unsigned queueSize_; 00093 00094 ros::NodeHandle& nodeHandle_; 00095 ros::NodeHandle& nodeHandlePrivate_; 00096 00097 image_transport::ImageTransport imageTransport_; 00098 00099 image_t image_; 00100 00101 std::string modelPath_; 00102 std::string modelName_; 00103 00104 std::string cameraPrefix_; 00105 std::string rectifiedImageTopic_; 00106 std::string cameraInfoTopic_; 00107 00108 boost::filesystem::path vrmlPath_; 00109 boost::filesystem::path initPath_; 00110 00111 image_transport::CameraSubscriber cameraSubscriber_; 00112 00113 boost::recursive_mutex mutex_; 00114 reconfigureSrv_t reconfigureSrv_; 00115 00116 std_msgs::Header header_; 00117 sensor_msgs::CameraInfoConstPtr info_; 00118 00119 vpMe movingEdge_; 00120 vpCameraParameters cameraParameters_; 00121 vpMbEdgeTracker tracker_; 00122 00123 bool startFromSavedPose_; 00124 bool confirmInit_; 00125 00127 image_proc::AdvertisementChecker checkInputs_; 00128 00129 resource_retriever::Retriever resourceRetriever_; 00130 }; 00131 } // end of namespace visp_tracker. 00132 00133 #endif //! VISP_TRACKER_TRACKER_CLIENT_HH