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/MovingEdgeConfig.h> 00022 # include <visp_tracker/MovingEdgeSites.h> 00023 00024 # include <visp/vpCameraParameters.h> 00025 # include <visp/vpHomogeneousMatrix.h> 00026 # include <visp/vpImage.h> 00027 # include <visp/vpMbEdgeTracker.h> 00028 00029 namespace visp_tracker 00030 { 00031 class Tracker 00032 { 00033 public: 00034 typedef vpImage<unsigned char> image_t; 00035 00036 typedef dynamic_reconfigure::Server<visp_tracker::MovingEdgeConfig> 00037 reconfigureSrv_t; 00038 00039 typedef boost::function<bool (visp_tracker::Init::Request&, 00040 visp_tracker::Init::Response& res)> 00041 initCallback_t; 00042 00043 enum State 00044 { 00045 WAITING_FOR_INITIALIZATION, 00046 TRACKING, 00047 LOST 00048 }; 00049 00050 00051 Tracker (ros::NodeHandle& nh, 00052 ros::NodeHandle& privateNh, 00053 volatile bool& exiting, 00054 unsigned queueSize = 5u); 00055 void spin(); 00056 protected: 00057 bool initCallback(visp_tracker::Init::Request& req, 00058 visp_tracker::Init::Response& res); 00059 00060 void updateMovingEdgeSites(visp_tracker::MovingEdgeSitesPtr sites); 00061 00062 void checkInputs(); 00063 void waitForImage(); 00064 00065 void objectPositionHintCallback 00066 (const geometry_msgs::TransformStampedConstPtr&); 00067 private: 00068 bool exiting () 00069 { 00070 return exiting_ || !ros::ok(); 00071 } 00072 00073 void spinOnce () 00074 { 00075 //callbackQueue_.callAvailable(ros::WallDuration(0)); 00076 ros::spinOnce (); 00077 } 00078 00079 volatile bool& exiting_; 00080 00081 unsigned queueSize_; 00082 00083 ros::NodeHandle& nodeHandle_; 00084 ros::NodeHandle& nodeHandlePrivate_; 00085 image_transport::ImageTransport imageTransport_; 00086 00087 State state_; 00088 00089 image_t image_; 00090 00091 std::string cameraPrefix_; 00092 std::string rectifiedImageTopic_; 00093 std::string cameraInfoTopic_; 00094 00095 boost::filesystem::path vrmlPath_; 00096 00097 image_transport::CameraSubscriber cameraSubscriber_; 00098 00099 boost::recursive_mutex mutex_; 00100 reconfigureSrv_t reconfigureSrv_; 00101 00102 ros::Publisher resultPublisher_; 00103 ros::Publisher transformationPublisher_; 00104 tf::TransformBroadcaster tfBroadcaster_; 00105 ros::Publisher movingEdgeSitesPublisher_; 00106 00107 ros::ServiceServer initService_; 00108 00109 std_msgs::Header header_; 00110 sensor_msgs::CameraInfoConstPtr info_; 00111 00112 vpMe movingEdge_; 00113 vpCameraParameters cameraParameters_; 00114 vpMbEdgeTracker tracker_; 00115 00116 unsigned lastTrackedImage_; 00117 00119 image_proc::AdvertisementChecker checkInputs_; 00120 00121 vpHomogeneousMatrix cMo_; 00122 00123 tf::TransformListener listener_; 00124 std::string worldFrameId_; 00125 bool compensateRobotMotion_; 00126 00127 tf::TransformBroadcaster transformBroadcaster_; 00128 std::string childFrameId_; 00129 00130 ros::Subscriber objectPositionHintSubscriber_; 00131 geometry_msgs::TransformStamped objectPositionHint_; 00132 }; 00133 } // end of namespace visp_tracker. 00134 00135 #endif //! VISP_TRACKER_TRACKER_HH