コード例 #1
0
DotTracker::DotTracker(unsigned queueSize)
    : queueSize_(queueSize),
      nodeHandle_(),
      imageTransport_(nodeHandle_),
      image_(),
      rectifiedImageTopic_(),
      cameraInfoTopic_(),
      pointCorrespondenceTopic_(),
      model_prefix_(),
      cameraSubscriber_(),
      pointCorrespondenceSubscriber_(),
      pointMsg_(),
	  points_(),
	  trackers_(),
	  trackers_status_(),
	  homography_(),
	  homography_pub_()
      
{
	
	homography_ = cvCreateMat(3, 3, CV_64FC1);
	homography_pub_ =  nodeHandle_.advertise<std_msgs::Float64MultiArray>("homography", 10);
	
	// find camera prefix name
	ros::Rate rate (1);
    while (rectifiedImageTopic_.empty ())
      {
	ros::param::get ("DotTracker/camera_topic", rectifiedImageTopic_);
	ros::param::get ("DotTracker/camera_info_topic", cameraInfoTopic_);
	ros::param::get ("DotTracker/point_correspondence_topic", pointCorrespondenceTopic_);
	ros::param::get ("DotTracker/model_prefix", model_prefix_);
	if (!ros::param::has ("DotTracker/camera_topic"))
	  {
	    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 (rectifiedImageTopic_.empty ())
	  {
	    ROS_INFO
	      ("tracker is not yet initialized, waiting...\n"
	       "You may want to launch the client to initialize the tracker.");
	  }
	if (!ros::ok ())
	  return;
	rate.sleep ();
      }
	      
	ROS_INFO_STREAM("camera_topic is " << rectifiedImageTopic_);
	ROS_INFO_STREAM("camera_info_topic is " << cameraInfoTopic_);
	ROS_INFO_STREAM("point_correspondence_topic is " << pointCorrespondenceTopic_);
	
	
	cameraSubscriber_ = imageTransport_.subscribeCamera
      (rectifiedImageTopic_, queueSize_,
       bindImageCallback(image_, header_, info_));
    //cameraSubscriber_ = imageTransport_.subscribe(rectifiedImageTopic_, queueSize_, imageCallback);
    // Wait for the image to be initialized.
     
    //register callback for pointCorrespondance 
    pointCorrespondenceSubscriber_ = nodeHandle_.subscribe(pointCorrespondenceTopic_, 1, &DotTracker::pointCorrespondenceCallback, this);
    //nodeHandle_.subscribe(pointCorrespondenceTopic_, 1, pointCorrespondenceCallbackGlobal);
    
     
    waitForImage();
    
    waitForInit();
}
コード例 #2
0
ファイル: tracker.cpp プロジェクト: HRZaheri/vision_visp
  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);
  }