node.cpp

Go to the documentation of this file.

#include "node.h"
#include "names.h"

//command line parameters
#include "cmd_line/cmd_line.h"


//tracking
#include "libauto_tracker/tracking.h"
#include "libauto_tracker/threading.h"
#include "libauto_tracker/events.h"

#include "visp_tracker/MovingEdgeSites.h"
#include "visp_tracker/KltPoints.h"

//visp includes
#include <visp3/gui/vpDisplayX.h>
#include <visp3/mbt/vpMbGenericTracker.h>
#include <visp3/core/vpTime.h>

//detectors
#if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
#  include "detectors/datamatrix/detector.h"
#  include "detectors/qrcode/detector.h"
#else
#  include <visp3/detection/vpDetectorDataMatrixCode.h>
#  include <visp3/detection/vpDetectorQRCode.h>
#endif

#include <visp_bridge/camera.h>
#include <visp_bridge/image.h>
#include <visp_bridge/3dpose.h>

#include "libauto_tracker/tracking.h"

#include <resource_retriever/retriever.h>

#include "std_msgs/Int8.h"
#include "std_msgs/String.h"

namespace visp_auto_tracker{
  Node::Node() :
    n_("~"),
    queue_size_(1),
    tracker_config_path_(),
    model_description_(),
    model_path_(),
    model_name_(),
    code_message_(),
    tracker_ref_frame_(),
    debug_display_(false),
    I_(),
    image_header_(),
    got_image_(false),
    cam_(),
    t_(NULL) {
    //get the tracker configuration file
    //this file contains all of the tracker's parameters, they are not passed to ros directly.
    n_.param<std::string>("tracker_config_path", tracker_config_path_, "");
    n_.param<bool>("debug_display", debug_display_, false);
    std::string model_full_path;
    n_.param<std::string>("model_path", model_path_, "");
    n_.param<std::string>("model_name", model_name_, "");
    n_.param<std::string>("code_message", code_message_, "");
    n_.param<std::string>("tracker_ref_frame", tracker_ref_frame_, "/map");
    model_path_= model_path_[model_path_.length()-1]=='/'?model_path_:model_path_+std::string("/");
    model_full_path = model_path_+model_name_;
    tracker_config_path_ = model_full_path+".cfg";
    ROS_INFO("model full path=%s",model_full_path.c_str());

    //Parse command line arguments from config file (as ros param)
    cmd_.init(tracker_config_path_);
    cmd_.set_data_directory(model_path_); //force data path
    cmd_.set_pattern_name(model_name_); //force model name
    cmd_.set_show_fps(false);
    if (! code_message_.empty()) {
      ROS_WARN_STREAM("Track only code with message: \"" << code_message_ << "\"");
      cmd_.set_code_message(code_message_);
    }

    resource_retriever::Retriever r;
    resource_retriever::MemoryResource res;
    try {
      res = r.get(std::string("file://")+cmd_.get_mbt_cad_file());
    }
    catch(...) {
      ROS_ERROR_STREAM("Unable to read wrl or cao model file as resource: " << std::string("file://")+cmd_.get_mbt_cad_file());
    }

    model_description_.resize(res.size);
    for (unsigned int i=0; i < res.size; ++i)
      model_description_[i] = res.data.get()[i];

    ROS_INFO("Model content=%s",model_description_.c_str());

    n_.setParam ("/model_description", model_description_);
  }

  void Node::waitForImage(){
    while ( ros::ok ()){
      if(got_image_) return;
      ros::spinOnce();
    }
  }

  //records last recieved image
  void Node::frameCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& cam_info){
    boost::mutex::scoped_lock(lock_);
    image_header_ = image->header;
    I_ = visp_bridge::toVispImageRGBa(*image); //make sure the image isn't worked on by locking a mutex
    cam_ = visp_bridge::toVispCameraParameters(*cam_info);

    got_image_ = true;
  }

  void Node::spin(){

    if(cmd_.should_exit()) return; //exit if needed

    vpMbTracker* tracker; //mb-tracker will be chosen according to config

    //create display
    vpDisplayX* d = NULL;
    if(debug_display_)
      d = new vpDisplayX();

    //init detector based on user preference
#if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
    detectors::DetectorBase* detector = NULL;
    if (cmd_.get_detector_type() == CmdLine::ZBAR)
      detector = new detectors::qrcode::Detector;
    else if(cmd_.get_detector_type() == CmdLine::DMTX)
      detector = new detectors::datamatrix::Detector;
#else // ViSP >= 2.10.0. In that case we use the detectors from ViSP
    vpDetectorBase *detector = NULL;
    if (cmd_.get_detector_type() == CmdLine::ZBAR)
      detector = new vpDetectorQRCode;
    else if(cmd_.get_detector_type() == CmdLine::DMTX)
      detector = new vpDetectorDataMatrixCode;
#endif

    // Use the best tracker
    int trackerType = vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER;
    tracker = new vpMbGenericTracker(1, trackerType);
    tracker->setCameraParameters(cam_);
    tracker->setDisplayFeatures(true);

    //compile detectors and paramters into the automatic tracker.
    t_ = new tracking::Tracker(cmd_, detector, tracker, debug_display_);
    t_->start(); //start the state machine

    //subscribe to ros topics and prepare a publisher that will publish the pose
    message_filters::Subscriber<sensor_msgs::Image> raw_image_subscriber(n_, image_topic, queue_size_);
    message_filters::Subscriber<sensor_msgs::CameraInfo> camera_info_subscriber(n_, camera_info_topic, queue_size_);
    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> image_info_sync(raw_image_subscriber, camera_info_subscriber, queue_size_);
    image_info_sync.registerCallback(boost::bind(&Node::frameCallback,this, _1, _2));
    ros::Publisher object_pose_publisher = n_.advertise<geometry_msgs::PoseStamped>(object_position_topic, queue_size_);
    ros::Publisher object_pose_covariance_publisher = n_.advertise<geometry_msgs::PoseWithCovarianceStamped>(object_position_covariance_topic, queue_size_);
    ros::Publisher moving_edge_sites_publisher = n_.advertise<visp_tracker::MovingEdgeSites>(moving_edge_sites_topic, queue_size_);
    ros::Publisher klt_points_publisher = n_.advertise<visp_tracker::KltPoints>(klt_points_topic, queue_size_);
    ros::Publisher status_publisher = n_.advertise<std_msgs::Int8>(status_topic, queue_size_);
    ros::Publisher code_message_publisher = n_.advertise<std_msgs::String>(code_message_topic, queue_size_);

    //wait for an image to be ready
    waitForImage();
    {
      //when an image is ready tell the tracker to start searching for patterns
      boost::mutex::scoped_lock(lock_);
      if(debug_display_) {
        d->init(I_); //also init display
        vpDisplay::setTitle(I_, "visp_auto_tracker debug display");
      }

      t_->process_event(tracking::select_input(I_));
    }

    unsigned int iter=0;
    geometry_msgs::PoseStamped ps;
    geometry_msgs::PoseWithCovarianceStamped ps_cov;

    ros::Rate rate(30); //init 25fps publishing frequency
    while(ros::ok()){
      double t = vpTime::measureTimeMs();
      boost::mutex::scoped_lock(lock_);
      //process the new frame with the tracker
      t_->process_event(tracking::input_ready(I_,cam_,iter));
      //When the tracker is tracking, it's in the tracking::TrackModel state
      //Access this state and broadcast the pose
      tracking::TrackModel& track_model = t_->get_state<tracking::TrackModel&>();

      ps.pose = visp_bridge::toGeometryMsgsPose(track_model.cMo); //convert

      // Publish resulting pose.
      if (object_pose_publisher.getNumSubscribers       () > 0)
      {
        ps.header = image_header_;
        ps.header.frame_id = tracker_ref_frame_;
        object_pose_publisher.publish(ps);
      }

      // Publish resulting pose covariance matrix.
      if (object_pose_covariance_publisher.getNumSubscribers    () > 0)
      {
        ps_cov.pose.pose = ps.pose;
        ps_cov.header = image_header_;
        ps_cov.header.frame_id = tracker_ref_frame_;

        for (unsigned i = 0; i < track_model.covariance.getRows(); ++i)
        {
          for (unsigned j = 0; j < track_model.covariance.getCols(); ++j)
          {
            unsigned idx = i * track_model.covariance.getCols() + j;
            if (idx >= 36)
              continue;
            ps_cov.pose.covariance[idx] = track_model.covariance[i][j];
          }
        }

        object_pose_covariance_publisher.publish(ps_cov);
      }

      // Publish state machine status.
      if (status_publisher.getNumSubscribers    () > 0)
      {
        std_msgs::Int8 status;
        status.data = (unsigned char)(*(t_->current_state()));
        status_publisher.publish(status);
      }

      // Publish moving edge sites.
      if (moving_edge_sites_publisher.getNumSubscribers () > 0)
      {
        visp_tracker::MovingEdgeSitesPtr sites (new visp_tracker::MovingEdgeSites);
        // Test if we are in the state tracking::TrackModel. In that case the pose is good;
        // we can send the moving edges. Otherwise we send an empty list of features
        if (*(t_->current_state()) == 3) {
          t_->updateMovingEdgeSites(sites);
        }

        sites->header = image_header_;
        moving_edge_sites_publisher.publish(sites);
      }

      // Publish KLT points.
      if (klt_points_publisher.getNumSubscribers        () > 0)
      {
        visp_tracker::KltPointsPtr klt (new visp_tracker::KltPoints);
        // Test if we are in the state tracking::TrackModel. In that case the pose is good;
        // we can send the klt points. Otherwise we send an empty list of features
        if (*(t_->current_state()) == 3) {
          t_->updateKltPoints(klt);
        }
        klt->header = image_header_;
        klt_points_publisher.publish(klt);
      }

      if (code_message_publisher.getNumSubscribers() > 0)
      {
        std_msgs::String message;
        if (*(t_->current_state()) == 3) { // Tracking successful
#if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
          message.data = detector->get_message();
#else
          message.data = detector->getMessage( cmd_.get_code_message_index() );
#endif
        }
        else {
          message.data = std::string();
        }
        code_message_publisher.publish(message);
        ROS_INFO_STREAM("Code with message \"" <<  message.data << "\" under tracking");
      }

      ros::spinOnce();
      rate.sleep();
      if (cmd_.show_fps())
        std::cout << "Tracking done in " << vpTime::measureTimeMs() - t << " ms" << std::endl;
    }
    t_->process_event(tracking::finished());
    if(debug_display_) {
      delete d;
    }
    delete tracker;
  }
}