tracker.hh
Go to the documentation of this file.
00001 #ifndef VISP_TRACKER_TRACKER_HH
00002 # define VISP_TRACKER_TRACKER_HH
00003 # include <boost/filesystem/path.hpp>
00004 # include <boost/thread/recursive_mutex.hpp>
00005 
00006 # include <dynamic_reconfigure/server.h>
00007 
00008 # include <image_proc/advertisement_checker.h>
00009 
00010 # include <image_transport/image_transport.h>
00011 
00012 # include <geometry_msgs/TwistStamped.h>
00013 
00014 # include <sensor_msgs/Image.h>
00015 # include <sensor_msgs/CameraInfo.h>
00016 
00017 # include <tf/transform_broadcaster.h>
00018 # include <tf/transform_listener.h>
00019 
00020 # include <visp_tracker/Init.h>
00021 # include <visp_tracker/ModelBasedSettingsConfig.h>
00022 # include <visp_tracker/ModelBasedSettingsKltConfig.h>
00023 # include <visp_tracker/ModelBasedSettingsEdgeConfig.h>
00024 # include <visp_tracker/MovingEdgeSites.h>
00025 # include <visp_tracker/KltPoints.h>
00026 
00027 # include <visp/vpCameraParameters.h>
00028 # include <visp/vpHomogeneousMatrix.h>
00029 # include <visp/vpImage.h>
00030 
00031 # include <visp/vpMbTracker.h>
00032 # include <visp/vpMbEdgeKltTracker.h>
00033 
00034 # include <visp/vpMe.h>
00035 # include <string>
00036 
00037 namespace visp_tracker
00038 {
00039   class Tracker
00040   {
00041   public:
00042     typedef vpImage<unsigned char> image_t;
00043 
00044     typedef boost::function<bool (visp_tracker::Init::Request&,
00045           visp_tracker::Init::Response& res)>
00046     initCallback_t;
00047 
00048     template<class ConfigType>
00049     struct reconfigureSrvStruct{
00050       typedef dynamic_reconfigure::Server<ConfigType> reconfigureSrv_t;
00051     };
00052 
00053     enum State
00054       {
00055         WAITING_FOR_INITIALIZATION,
00056         TRACKING,
00057         LOST
00058       };
00059 
00060 
00061     Tracker (ros::NodeHandle& nh,
00062              ros::NodeHandle& privateNh,
00063              volatile bool& exiting,
00064              unsigned queueSize = 5u);
00065     
00066     ~Tracker();
00067     
00068     void spin();
00069   protected:
00070     bool initCallback(visp_tracker::Init::Request& req,
00071                       visp_tracker::Init::Response& res);
00072 
00073     void updateMovingEdgeSites(visp_tracker::MovingEdgeSitesPtr sites);
00074     void updateKltPoints(visp_tracker::KltPointsPtr klt);
00075 
00076     void checkInputs();
00077     void waitForImage();
00078 
00079     void objectPositionHintCallback
00080     (const geometry_msgs::TransformStampedConstPtr&);
00081   private:
00082     bool exiting ()
00083     {
00084       return exiting_ || !ros::ok();
00085     }
00086 
00087     void spinOnce ()
00088     {
00089       //callbackQueue_.callAvailable(ros::WallDuration(0));
00090       ros::spinOnce ();
00091     }
00092 
00093     volatile bool& exiting_;
00094 
00095     unsigned queueSize_;
00096 
00097     ros::NodeHandle& nodeHandle_;
00098     ros::NodeHandle& nodeHandlePrivate_;
00099     image_transport::ImageTransport imageTransport_;
00100 
00101     State state_;
00102     std::string trackerType_;
00103 
00104     image_t image_;
00105 
00106     std::string cameraPrefix_;
00107     std::string rectifiedImageTopic_;
00108     std::string cameraInfoTopic_;
00109 
00110     boost::filesystem::path modelPath_;
00111 
00112     image_transport::CameraSubscriber cameraSubscriber_;
00113 
00114     boost::recursive_mutex mutex_;
00115 
00116     reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t *reconfigureSrv_;
00117     reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t *reconfigureKltSrv_;
00118     reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t *reconfigureEdgeSrv_;
00119 
00120     ros::Publisher resultPublisher_;
00121     ros::Publisher transformationPublisher_;
00122     tf::TransformBroadcaster tfBroadcaster_;
00123     ros::Publisher movingEdgeSitesPublisher_;
00124     ros::Publisher kltPointsPublisher_;
00125 
00126     ros::ServiceServer initService_;
00127 
00128     std_msgs::Header header_;
00129     sensor_msgs::CameraInfoConstPtr info_;
00130 
00131     vpKltOpencv kltTracker_;
00132     vpMe movingEdge_;
00133     vpCameraParameters cameraParameters_;
00134     vpMbTracker* tracker_;
00135 
00136     unsigned lastTrackedImage_;
00137 
00139     image_proc::AdvertisementChecker checkInputs_;
00140 
00141     vpHomogeneousMatrix cMo_;
00142 
00143     tf::TransformListener listener_;
00144     std::string worldFrameId_;
00145     bool compensateRobotMotion_;
00146 
00147     tf::TransformBroadcaster transformBroadcaster_;
00148     std::string childFrameId_;
00149 
00150     ros::Subscriber objectPositionHintSubscriber_;
00151     geometry_msgs::TransformStamped objectPositionHint_;
00152   };
00153 } // end of namespace visp_tracker.
00154 
00155 #endif //! VISP_TRACKER_TRACKER_HH


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