tracker-viewer.hh

Go to the documentation of this file.

#ifndef VISP_TRACKER_TRACKER_VIEWER_HH
# define VISP_TRACKER_TRACKER_VIEWER_HH
# include <boost/filesystem/path.hpp>
# include <boost/optional.hpp>
 
# include <geometry_msgs/PoseWithCovarianceStamped.h>
 
# include <image_proc/advertisement_checker.h>
 
# include <image_transport/image_transport.h>
# include <image_transport/subscriber_filter.h>
 
# include <message_filters/subscriber.h>
# include <message_filters/sync_policies/approximate_time.h>
# include <message_filters/synchronizer.h>
 
# include <sensor_msgs/Image.h>
# include <sensor_msgs/CameraInfo.h>
 
# include <visp_tracker/Init.h>
# include <visp_tracker/MovingEdgeSites.h>
# include <visp_tracker/KltPoints.h>
 
# include <visp3/core/vpCameraParameters.h>
# include <visp3/core/vpImage.h>
# include <visp3/mbt/vpMbGenericTracker.h>
 
namespace visp_tracker
{
  class TrackerViewer
  {
  public:
    typedef vpImage<unsigned char> image_t;
 
 
    typedef boost::function<bool (visp_tracker::Init::Request&,
                                  visp_tracker::Init::Response& res)>
    initCallback_t;
 
    typedef boost::function<bool (visp_tracker::Init::Request&,
                                  visp_tracker::Init::Response& res)>
    reconfigureCallback_t;
 
    typedef message_filters::sync_policies::ApproximateTime<
    sensor_msgs::Image, sensor_msgs::CameraInfo,
    geometry_msgs::PoseWithCovarianceStamped,
    visp_tracker::MovingEdgeSites,
    visp_tracker::KltPoints
    > syncPolicy_t;
 
    TrackerViewer(ros::NodeHandle& nh,
                  ros::NodeHandle& privateNh,
                  volatile bool& exiting,
                  unsigned queueSize = 5u);
 
    void spin();
  protected:
    void initializeTracker();
 
    void loadCommonParameters();
 
    void checkInputs();
 
    void waitForImage();
 
    bool initCallback(visp_tracker::Init::Request& req,
                      visp_tracker::Init::Response& res);
 
    bool reconfigureCallback(visp_tracker::Init::Request& req,
                             visp_tracker::Init::Response& res);
 
    void
    callback
    (const sensor_msgs::ImageConstPtr& imageConst,
     const sensor_msgs::CameraInfoConstPtr& infoConst,
     const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult,
     const visp_tracker::MovingEdgeSites::ConstPtr& sitesConst,
     const visp_tracker::KltPoints::ConstPtr& kltConst);
 
    void timerCallback();
 
    void displayMovingEdgeSites();
    void displayKltPoints();
 
  private:
    bool exiting ()
    {
      return exiting_ || !ros::ok();
    }
 
    volatile bool& exiting_;
 
    unsigned queueSize_;
 
    ros::NodeHandle& nodeHandle_;
    ros::NodeHandle& nodeHandlePrivate_;
 
    image_transport::ImageTransport imageTransport_;
 
    double frameSize_;
 
 
    std::string rectifiedImageTopic_;
    std::string cameraInfoTopic_;
 
 
    ros::ServiceServer initService_;
 
    ros::ServiceServer reconfigureService_;
 
    std::string trackerName_;
 
    boost::filesystem::path modelPath_;
 
    image_proc::AdvertisementChecker checkInputs_;
 
    vpMbGenericTracker tracker_;
    vpCameraParameters cameraParameters_;
    image_t image_;
 
    sensor_msgs::CameraInfoConstPtr info_;
    boost::optional<vpHomogeneousMatrix> cMo_;
    visp_tracker::MovingEdgeSites::ConstPtr sites_;
    visp_tracker::KltPoints::ConstPtr klt_;
 
    image_transport::SubscriberFilter imageSubscriber_;
    message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSubscriber_;
    message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>
    trackingResultSubscriber_;
    message_filters::Subscriber<visp_tracker::MovingEdgeSites>
    movingEdgeSitesSubscriber_;
    message_filters::Subscriber<visp_tracker::KltPoints>
    kltPointsSubscriber_;
 
    message_filters::Synchronizer<syncPolicy_t> synchronizer_;
 
    ros::WallTimer timer_;
    unsigned countAll_;
    unsigned countImages_;
    unsigned countCameraInfo_;
    unsigned countTrackingResult_;
    unsigned countMovingEdgeSites_;
    unsigned countKltPoints_;
  };
} // end of namespace visp_tracker
 
#endif