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 <visp/vpCameraParameters.h>
# include <visp/vpImage.h>
# include <visp/vpMbEdgeTracker.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_;

    vpMbEdgeTracker 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 //! VISP_TRACKER_TRACKER_VIEWER_HH