TrackerViewer::TrackerViewer(ros::NodeHandle& nh,
			       ros::NodeHandle& privateNh,
			       volatile bool& exiting,
			       unsigned queueSize)
    : exiting_ (exiting),
      queueSize_(queueSize),
      nodeHandle_(nh),
      nodeHandlePrivate_(privateNh),
      imageTransport_(nodeHandle_),
      rectifiedImageTopic_(),
      cameraInfoTopic_(),
      checkInputs_(nodeHandle_, ros::this_node::getName()),
      tracker_(),
      cameraParameters_(),
      image_(),
      info_(),
      cMo_(boost::none),
      sites_(),
      imageSubscriber_(),
      cameraInfoSubscriber_(),
      trackingResultSubscriber_(),
      movingEdgeSitesSubscriber_(),
      kltPointsSubscriber_(),
      synchronizer_(syncPolicy_t(queueSize_)),
      timer_(),
      countAll_(0u),
      countImages_(0u),
      countCameraInfo_(0u),
      countTrackingResult_(0u),
      countMovingEdgeSites_(0u),
      countKltPoints_(0u)
  {
    // Compute topic and services names.
    std::string cameraPrefix;

    ros::Rate rate (1);
    while (cameraPrefix.empty ())
      {
	if (!nodeHandle_.getParam ("camera_prefix", cameraPrefix))
	  {
	    ROS_WARN
	      ("the camera_prefix parameter does not exist.\n"
	       "This may mean that:\n"
	       "- the tracker is not launched,\n"
	       "- the tracker and viewer are not running in the same namespace."
	       );
	  }
	else if (cameraPrefix.empty ())
	  {
	    ROS_INFO
	      ("tracker is not yet initialized, waiting...\n"
	       "You may want to launch the client to initialize the tracker.");
	  }
	if (this->exiting())
	  return;
	rate.sleep ();
      }

    rectifiedImageTopic_ =
      ros::names::resolve(cameraPrefix + "/image_rect");
    cameraInfoTopic_ =
      ros::names::resolve(cameraPrefix + "/camera_info");

    boost::filesystem::ofstream modelStream;
    std::string path;

    while (!nodeHandle_.hasParam(visp_tracker::model_description_param))
      {
	if (!nodeHandle_.hasParam(visp_tracker::model_description_param))
	  {
	    ROS_WARN
	      ("the model_description parameter does not exist.\n"
	       "This may mean that:\n"
	       "- the tracker is not launched or not initialized,\n"
	       "- the tracker and viewer are not running in the same namespace."
	       );
	  }
	if (this->exiting())
	  return;
	rate.sleep ();
      }

    if (!makeModelFile(modelStream, path))
      throw std::runtime_error
	("failed to load the model from the parameter server");
    ROS_INFO_STREAM("Model loaded from the parameter server.");
    vrmlPath_ = path;

    initializeTracker();
    if (this->exiting())
      return;

    checkInputs();
    if (this->exiting())
      return;

    // Subscribe to camera and tracker synchronously.
    imageSubscriber_.subscribe
      (imageTransport_, rectifiedImageTopic_, queueSize_);
    cameraInfoSubscriber_.subscribe
      (nodeHandle_, cameraInfoTopic_, queueSize_);
    trackingResultSubscriber_.subscribe
      (nodeHandle_, visp_tracker::object_position_covariance_topic,
       queueSize_);
    movingEdgeSitesSubscriber_.subscribe
      (nodeHandle_, visp_tracker::moving_edge_sites_topic, queueSize_);
    kltPointsSubscriber_.subscribe
      (nodeHandle_, visp_tracker::klt_points_topic, queueSize_);

    synchronizer_.connectInput
      (imageSubscriber_, cameraInfoSubscriber_,
       trackingResultSubscriber_, movingEdgeSitesSubscriber_, kltPointsSubscriber_);
    synchronizer_.registerCallback(boost::bind(&TrackerViewer::callback,
                 this, _1, _2, _3, _4, _5));

    // Check for synchronization every 30s.
    synchronizer_.registerCallback(boost::bind(increment, &countAll_));
    imageSubscriber_.registerCallback(boost::bind(increment, &countImages_));
    cameraInfoSubscriber_.registerCallback
      (boost::bind(increment, &countCameraInfo_));
    trackingResultSubscriber_.registerCallback
      (boost::bind(increment, &countTrackingResult_));
    movingEdgeSitesSubscriber_.registerCallback
      (boost::bind(increment, &countMovingEdgeSites_));
    kltPointsSubscriber_.registerCallback
      (boost::bind(increment, &countKltPoints_));

    timer_ = nodeHandle_.createWallTimer
      (ros::WallDuration(30.),
       boost::bind(&TrackerViewer::timerCallback, this));

    // Wait for image.
    waitForImage();
    if (this->exiting())
      return;
    if (!image_.getWidth() || !image_.getHeight())
      throw std::runtime_error("failed to retrieve image");

    // Load camera parameters.
    initializeVpCameraFromCameraInfo(cameraParameters_, info_);
    tracker_.setCameraParameters(cameraParameters_);
  }
Beispiel #2
0
  Tracker::Tracker(ros::NodeHandle& nh,
		   ros::NodeHandle& privateNh,
		   volatile bool& exiting,
		   unsigned queueSize)
    : exiting_ (exiting),
      queueSize_(queueSize),
      nodeHandle_(nh),
      nodeHandlePrivate_(privateNh),
      imageTransport_(nodeHandle_),
      state_(WAITING_FOR_INITIALIZATION),
      image_(),
      cameraPrefix_(),
      rectifiedImageTopic_(),
      cameraInfoTopic_(),
      modelPath_(),
      cameraSubscriber_(),
      mutex_ (),
      //reconfigureSrv_(mutex_, nodeHandlePrivate_),
      reconfigureSrv_(NULL),
      reconfigureKltSrv_(NULL),
      reconfigureEdgeSrv_(NULL),
      resultPublisher_(),
      transformationPublisher_(),
      movingEdgeSitesPublisher_(),
      kltPointsPublisher_(),
      initService_(),
      header_(),
      info_(),
      kltTracker_(),
      movingEdge_(),
      cameraParameters_(),
      lastTrackedImage_(),
      checkInputs_(nodeHandle_, ros::this_node::getName()),
      cMo_ (),
      listener_ (),
      worldFrameId_ (),
      compensateRobotMotion_ (false),
      transformBroadcaster_ (),
      childFrameId_ (),
      objectPositionHintSubscriber_ (),
      objectPositionHint_ ()
  {
    // Set cMo to identity.
    cMo_.eye();
    //tracker_ = new vpMbEdgeTracker();

    // Parameters.
    nodeHandlePrivate_.param<std::string>("camera_prefix", cameraPrefix_, "");
    nodeHandlePrivate_.param<std::string>("tracker_type", trackerType_, "mbt");
    if(trackerType_=="mbt")
      tracker_ = new vpMbEdgeTracker();
    else if(trackerType_=="klt")
      tracker_ = new vpMbKltTracker();
    else
      tracker_ = new vpMbEdgeKltTracker();

    if (cameraPrefix_.empty ())
      {
	ROS_FATAL
	  ("The camera_prefix parameter not set.\n"
	   "Please relaunch the tracker while setting this parameter, i.e.\n"
	   "$ rosrun visp_tracker tracker _camera_prefix:=/my/camera");
	ros::shutdown ();
	return;
      }
    // Create global /camera_prefix param to avoid to remap in the launch files the tracker_client and tracker_viewer nodes
    nodeHandle_.setParam("camera_prefix", cameraPrefix_);

    nodeHandle_.param<std::string>("frame_id", childFrameId_, "object_position");

    // Robot motion compensation.
    nodeHandle_.param<std::string>("world_frame_id", worldFrameId_, "/odom");
    nodeHandle_.param<bool>
      ("compensate_robot_motion", compensateRobotMotion_, false);

    // Compute topic and services names.
    rectifiedImageTopic_ =
      ros::names::resolve(cameraPrefix_ + "/image_rect");

    // Check for subscribed topics.
    checkInputs();

    // Result publisher.
    resultPublisher_ =
      nodeHandle_.advertise<geometry_msgs::PoseWithCovarianceStamped>
      (visp_tracker::object_position_covariance_topic, queueSize_);

    transformationPublisher_ =
      nodeHandle_.advertise<geometry_msgs::TransformStamped>
      (visp_tracker::object_position_topic, queueSize_);

    // Moving edge sites_ publisher.
    movingEdgeSitesPublisher_ =
      nodeHandle_.advertise<visp_tracker::MovingEdgeSites>
      (visp_tracker::moving_edge_sites_topic, queueSize_);

    // Klt_points_ publisher.
    kltPointsPublisher_ =
      nodeHandle_.advertise<visp_tracker::KltPoints>
      (visp_tracker::klt_points_topic, queueSize_);

    // Camera subscriber.
    cameraSubscriber_ =
      imageTransport_.subscribeCamera
      (rectifiedImageTopic_, queueSize_,
       bindImageCallback(image_, header_, info_));

    // Object position hint subscriber.
    typedef boost::function<
    void (const geometry_msgs::TransformStampedConstPtr&)>
      objectPositionHintCallback_t;
    objectPositionHintCallback_t callback =
      boost::bind (&Tracker::objectPositionHintCallback, this, _1);
    objectPositionHintSubscriber_ =
      nodeHandle_.subscribe<geometry_msgs::TransformStamped>
      ("object_position_hint", queueSize_, callback);

    // Initialization.
    // No more necessary as it is done via the reconfigure server
//    movingEdge_.initMask();
//    if(trackerType_!="klt"){
//      vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker_);
//      t->setMovingEdge(movingEdge_);
//    }
    
//    if(trackerType_!="mbt"){
//      vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker_);
//      t->setKltOpencv(kltTracker_);
//    }
    
    // Dynamic reconfigure.
    if(trackerType_=="mbt+klt"){ // Hybrid Tracker reconfigure
      reconfigureSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
      reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t::CallbackType f =
        boost::bind(&reconfigureCallbackAndInitViewer,
                    boost::ref(nodeHandle_), boost::ref(tracker_),
                    boost::ref(image_), boost::ref(movingEdge_), boost::ref(kltTracker_),
                    boost::ref(mutex_), _1, _2);
      reconfigureSrv_->setCallback(f);
    }
    else if(trackerType_=="mbt"){ // Edge Tracker reconfigure
      reconfigureEdgeSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
      reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t::CallbackType f =
        boost::bind(&reconfigureEdgeCallbackAndInitViewer,
                    boost::ref(nodeHandle_), boost::ref(tracker_),
                    boost::ref(image_), boost::ref(movingEdge_),
                    boost::ref(mutex_), _1, _2);
      reconfigureEdgeSrv_->setCallback(f);
    }
    else{ // KLT Tracker reconfigure
      reconfigureKltSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
      reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t::CallbackType f =
        boost::bind(&reconfigureKltCallbackAndInitViewer,
                    boost::ref(nodeHandle_), boost::ref(tracker_),
                    boost::ref(image_), boost::ref(kltTracker_),
                    boost::ref(mutex_), _1, _2);
      reconfigureKltSrv_->setCallback(f);
    }
    
    // Wait for the image to be initialized.
    waitForImage();
    if (this->exiting())
      return;
    if (!image_.getWidth() || !image_.getHeight())
      throw std::runtime_error("failed to retrieve image");

    // Tracker initialization.
    initializeVpCameraFromCameraInfo(cameraParameters_, info_);

    // Double check camera parameters.
    if (cameraParameters_.get_px () == 0.
	|| cameraParameters_.get_px () == 1.
	|| cameraParameters_.get_py () == 0.
	|| cameraParameters_.get_py () == 1.
	|| cameraParameters_.get_u0 () == 0.
	|| cameraParameters_.get_u0 () == 1.
	|| cameraParameters_.get_v0 () == 0.
	|| cameraParameters_.get_v0 () == 1.)
      ROS_WARN ("Dubious camera parameters detected.\n"
		"\n"
		"It seems that the matrix P from your camera\n"
		"calibration topics is wrong.\n"
		"The tracker will continue anyway, but you\n"
		"should double check your calibration data,\n"
		"especially if the model re-projection fails.\n"
		"\n"
		"This warning is triggered is px, py, u0 or v0\n"
		"is set to 0. or 1. (exactly).");

    tracker_->setCameraParameters(cameraParameters_);
    tracker_->setDisplayFeatures(false);

    ROS_INFO_STREAM(cameraParameters_);

    // Service declaration.
    initCallback_t initCallback =
      boost::bind(&Tracker::initCallback, this, _1, _2);

    initService_ = nodeHandle_.advertiseService
      (visp_tracker::init_service, initCallback);
  }