tracker.hh
Go to the documentation of this file.00001 #ifndef VISP_TRACKER_TRACKER_HH 00002 # define VISP_TRACKER_TRACKER_HH 00003 # include <boost/circular_buffer.hpp> 00004 # include <boost/filesystem/path.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 00019 # include <visp_tracker/Init.h> 00020 # include <visp_tracker/MovingEdgeConfig.h> 00021 # include <visp_tracker/MovingEdgeSites.h> 00022 # include <visp_tracker/TrackingResult.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(unsigned queueSize = 5u); 00052 void spin(); 00053 protected: 00054 bool initCallback(visp_tracker::Init::Request& req, 00055 visp_tracker::Init::Response& res); 00056 void 00057 cameraVelocityCallback(const geometry_msgs::TwistStampedConstPtr& twist); 00058 00059 void updateMovingEdgeSites(); 00060 00061 void checkInputs(); 00062 void waitForImage(); 00063 00064 void integrateCameraVelocity(const std_msgs::Header& lastHeader, 00065 const std_msgs::Header& currentHeader); 00066 00067 std::string velocitiesDebugMessage(); 00068 private: 00069 typedef std::pair<double, vpColVector> velocityDuringInterval_t; 00070 typedef boost::circular_buffer<velocityDuringInterval_t> velocities_t; 00071 static const velocities_t::size_type MAX_VELOCITY_VALUES = 1000; 00072 00073 unsigned queueSize_; 00074 00075 ros::NodeHandle nodeHandle_; 00076 image_transport::ImageTransport imageTransport_; 00077 00078 State state_; 00079 00080 image_t image_; 00081 00082 std::string cameraPrefix_; 00083 std::string rectifiedImageTopic_; 00084 std::string cameraInfoTopic_; 00085 00086 boost::filesystem::path vrmlPath_; 00087 00088 image_transport::CameraSubscriber cameraSubscriber_; 00089 00090 reconfigureSrv_t reconfigureSrv_; 00091 ros::Publisher resultPublisher_; 00092 ros::Publisher transformationPublisher_; 00093 tf::TransformBroadcaster tfBroadcaster_; 00094 ros::Publisher movingEdgeSitesPublisher_; 00095 ros::Subscriber cameraVelocitySubscriber_; 00096 00097 ros::ServiceServer initService_; 00098 00099 std_msgs::Header header_; 00100 sensor_msgs::CameraInfoConstPtr info_; 00101 00102 vpMe movingEdge_; 00103 vpCameraParameters cameraParameters_; 00104 vpMbEdgeTracker tracker_; 00105 00106 visp_tracker::MovingEdgeSites sites_; 00107 00108 unsigned lastTrackedImage_; 00109 00111 image_proc::AdvertisementChecker checkInputs_; 00112 00113 vpHomogeneousMatrix cMo_; 00114 00115 velocities_t velocities_; 00116 00117 tf::TransformBroadcaster transformBroadcaster_; 00118 std::string childFrameId_; 00119 }; 00120 } // end of namespace visp_tracker. 00121 00122 #endif //! VISP_TRACKER_TRACKER_HH