tracker.hh

Go to the documentation of this file.

#ifndef VISP_TRACKER_TRACKER_HH
# define VISP_TRACKER_TRACKER_HH
# include <boost/filesystem/path.hpp>
# include <boost/thread/recursive_mutex.hpp>

# include <dynamic_reconfigure/server.h>

# include <image_proc/advertisement_checker.h>

# include <image_transport/image_transport.h>

# include <geometry_msgs/TwistStamped.h>

# include <sensor_msgs/Image.h>
# include <sensor_msgs/CameraInfo.h>

# include <tf/transform_broadcaster.h>
# include <tf/transform_listener.h>

# include <visp_tracker/Init.h>
# include <visp_tracker/ModelBasedSettingsConfig.h>
# include <visp_tracker/ModelBasedSettingsKltConfig.h>
# include <visp_tracker/ModelBasedSettingsEdgeConfig.h>
# include <visp_tracker/MovingEdgeSites.h>
# include <visp_tracker/KltPoints.h>

# include <visp3/core/vpCameraParameters.h>
# include <visp3/core/vpHomogeneousMatrix.h>
# include <visp3/core/vpImage.h>
# include <visp3/mbt/vpMbGenericTracker.h>
# include <visp3/me/vpMe.h>

# include <string>

namespace visp_tracker
{
  class Tracker
  {
  public:
    typedef vpImage<unsigned char> image_t;

    typedef boost::function<bool (visp_tracker::Init::Request&,
                                  visp_tracker::Init::Response& res)>
    initCallback_t;

    template<class ConfigType>
    struct reconfigureSrvStruct{
      typedef dynamic_reconfigure::Server<ConfigType> reconfigureSrv_t;
    };

    enum State
    {
      WAITING_FOR_INITIALIZATION,
      TRACKING,
      LOST
    };


    Tracker (ros::NodeHandle& nh,
             ros::NodeHandle& privateNh,
             volatile bool& exiting,
             unsigned queueSize = 5u);
    
    ~Tracker();
    
    void spin();
  protected:
    bool initCallback(visp_tracker::Init::Request& req,
                      visp_tracker::Init::Response& res);

    void updateMovingEdgeSites(visp_tracker::MovingEdgeSitesPtr sites);
    void updateKltPoints(visp_tracker::KltPointsPtr klt);

    void checkInputs();
    void waitForImage();

    void objectPositionHintCallback
    (const geometry_msgs::TransformStampedConstPtr&);
  private:
    bool exiting ()
    {
      return exiting_ || !ros::ok();
    }

    void spinOnce ()
    {
      //callbackQueue_.callAvailable(ros::WallDuration(0));
      ros::spinOnce ();
    }

    volatile bool& exiting_;

    unsigned queueSize_;

    ros::NodeHandle& nodeHandle_;
    ros::NodeHandle& nodeHandlePrivate_;
    image_transport::ImageTransport imageTransport_;

    State state_;
    std::string trackerType_;

    image_t image_;

    std::string cameraPrefix_;
    std::string rectifiedImageTopic_;
    std::string cameraInfoTopic_;

    boost::filesystem::path modelPath_;

    image_transport::CameraSubscriber cameraSubscriber_;

    boost::recursive_mutex mutex_;

    reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t *reconfigureSrv_;
    reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t *reconfigureKltSrv_;
    reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t *reconfigureEdgeSrv_;

    ros::Publisher resultPublisher_;
    ros::Publisher transformationPublisher_;
    tf::TransformBroadcaster tfBroadcaster_;
    ros::Publisher movingEdgeSitesPublisher_;
    ros::Publisher kltPointsPublisher_;

    ros::ServiceServer initService_;
    std_msgs::Header header_;
    sensor_msgs::CameraInfoConstPtr info_;

    vpKltOpencv kltTracker_;
    vpMe movingEdge_;
    vpCameraParameters cameraParameters_;
    vpMbGenericTracker tracker_;

    unsigned lastTrackedImage_;

    image_proc::AdvertisementChecker checkInputs_;

    vpHomogeneousMatrix cMo_;

    tf::TransformListener listener_;
    std::string worldFrameId_;
    bool compensateRobotMotion_;

    tf::TransformBroadcaster transformBroadcaster_;
    std::string childFrameId_;

    ros::Subscriber objectPositionHintSubscriber_;
    geometry_msgs::TransformStamped objectPositionHint_;
  };
} // end of namespace visp_tracker.

#endif