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/MovingEdgeSites.h> 00021 00022 # include <visp/vpCameraParameters.h> 00023 # include <visp/vpImage.h> 00024 # include <visp/vpMbEdgeTracker.h> 00025 00026 namespace visp_tracker 00027 { 00029 class TrackerViewer 00030 { 00031 public: 00033 typedef vpImage<unsigned char> image_t; 00034 00042 typedef message_filters::sync_policies::ApproximateTime< 00043 sensor_msgs::Image, sensor_msgs::CameraInfo, 00044 geometry_msgs::PoseWithCovarianceStamped, 00045 visp_tracker::MovingEdgeSites 00046 > syncPolicy_t; 00047 00049 TrackerViewer(ros::NodeHandle& nh, 00050 ros::NodeHandle& privateNh, 00051 volatile bool& exiting, 00052 unsigned queueSize = 5u); 00053 00056 void spin(); 00057 protected: 00059 void initializeTracker(); 00060 00062 void checkInputs(); 00063 00065 void waitForImage(); 00066 00068 void 00069 callback 00070 (const sensor_msgs::ImageConstPtr& imageConst, 00071 const sensor_msgs::CameraInfoConstPtr& infoConst, 00072 const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult, 00073 const visp_tracker::MovingEdgeSites::ConstPtr& sitesConst); 00074 00075 void timerCallback(); 00076 00078 void displayMovingEdgeSites(); 00079 00080 private: 00081 bool exiting () 00082 { 00083 return exiting_ || !ros::ok(); 00084 } 00085 00086 volatile bool& exiting_; 00087 00089 unsigned queueSize_; 00090 00091 ros::NodeHandle& nodeHandle_; 00092 ros::NodeHandle& nodeHandlePrivate_; 00093 00095 image_transport::ImageTransport imageTransport_; 00096 00097 00100 00102 std::string rectifiedImageTopic_; 00104 std::string cameraInfoTopic_; 00105 00107 00108 00110 boost::filesystem::path vrmlPath_; 00111 00113 image_proc::AdvertisementChecker checkInputs_; 00114 00116 vpMbEdgeTracker tracker_; 00118 vpCameraParameters cameraParameters_; 00120 image_t image_; 00121 00123 sensor_msgs::CameraInfoConstPtr info_; 00125 boost::optional<vpHomogeneousMatrix> cMo_; 00127 visp_tracker::MovingEdgeSites::ConstPtr sites_; 00128 00132 image_transport::SubscriberFilter imageSubscriber_; 00134 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSubscriber_; 00136 message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> 00137 trackingResultSubscriber_; 00139 message_filters::Subscriber<visp_tracker::MovingEdgeSites> 00140 movingEdgeSitesSubscriber_; 00141 00143 message_filters::Synchronizer<syncPolicy_t> synchronizer_; 00145 00148 ros::WallTimer timer_; 00149 unsigned countAll_; 00150 unsigned countImages_; 00151 unsigned countCameraInfo_; 00152 unsigned countTrackingResult_; 00153 unsigned countMovingEdgeSites_; 00155 }; 00156 } // end of namespace visp_tracker 00157 00158 #endif //! VISP_TRACKER_TRACKER_VIEWER_HH