tracker-viewer.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <fstream>
00003 #include <sstream>
00004 
00005 #include <boost/bind.hpp>
00006 #include <boost/filesystem.hpp>
00007 #include <boost/filesystem/fstream.hpp>
00008 #include <boost/filesystem/path.hpp>
00009 #include <boost/format.hpp>
00010 #include <boost/optional.hpp>
00011 
00012 #include <ros/ros.h>
00013 #include <ros/param.h>
00014 #include <image_transport/image_transport.h>
00015 
00016 #include <visp/vpDisplayX.h>
00017 
00018 #include "conversion.hh"
00019 #include "callbacks.hh"
00020 #include "file.hh"
00021 #include "names.hh"
00022 
00023 #include "tracker-viewer.hh"
00024 
00025 namespace visp_tracker
00026 {
00027   namespace
00028   {
00029     static void increment(unsigned int* value)
00030     {
00031       ++(*value);
00032     }
00033   } // end of anonymous namespace.
00034 
00035   // Callback to fix ROS bug when /model_description (not properly cleared) doesn't contain the right model of the object to track.
00036   // Bug only occurs when viewer is started too early and with a different model than the previous call.
00037   bool
00038   TrackerViewer::initCallback(visp_tracker::Init::Request& req,
00039       visp_tracker::Init::Response& res)
00040   {
00041     boost::filesystem::ofstream modelStream;
00042     std::string path;
00043 
00044     if (!makeModelFile(modelStream, path))
00045       throw std::runtime_error("failed to load the model from the callback");
00046     //ROS_WARN_STREAM("Make model Viewer: " << path.c_str());
00047     ROS_INFO_STREAM("Model loaded from the service.");
00048     modelPath_ = path;
00049 
00050     tracker_.resetTracker();
00051     initializeTracker();
00052 
00053     // Common parameters
00054     convertInitRequestToVpMbTracker(req, &tracker_);
00055 
00056     res.initialization_succeed = true;
00057     return true;
00058   }
00059 
00060   bool
00061   TrackerViewer::reconfigureCallback(visp_tracker::Init::Request& req,
00062       visp_tracker::Init::Response& res)
00063   {
00064     // Common parameters
00065     ROS_INFO_STREAM("Reconfiguring Tracker Viewer.");
00066     convertInitRequestToVpMbTracker(req, &tracker_);
00067 
00068     res.initialization_succeed = true;
00069     return true;
00070   }
00071 
00072   TrackerViewer::TrackerViewer(ros::NodeHandle& nh,
00073                                ros::NodeHandle& privateNh,
00074                                volatile bool& exiting,
00075                                unsigned queueSize)
00076     : exiting_ (exiting),
00077       queueSize_(queueSize),
00078       nodeHandle_(nh),
00079       nodeHandlePrivate_(privateNh),
00080       imageTransport_(nodeHandle_),
00081       frameSize_(0.1),
00082       rectifiedImageTopic_(),
00083       cameraInfoTopic_(),
00084       checkInputs_(nodeHandle_, ros::this_node::getName()),
00085       tracker_(),
00086       cameraParameters_(),
00087       image_(),
00088       info_(),
00089       initService_(),
00090       reconfigureService_(),
00091       trackerName_(),
00092       cMo_(boost::none),
00093       sites_(),
00094       imageSubscriber_(),
00095       cameraInfoSubscriber_(),
00096       trackingResultSubscriber_(),
00097       movingEdgeSitesSubscriber_(),
00098       kltPointsSubscriber_(),
00099       synchronizer_(syncPolicy_t(queueSize_)),
00100       timer_(),
00101       countAll_(0u),
00102       countImages_(0u),
00103       countCameraInfo_(0u),
00104       countTrackingResult_(0u),
00105       countMovingEdgeSites_(0u),
00106       countKltPoints_(0u)
00107   {
00108     // Compute topic and services names.
00109     std::string cameraPrefix;
00110 
00111     ros::Rate rate (1);
00112     while (cameraPrefix.empty ())
00113       {
00114       // Check for the global parameter /camera_prefix set by visp_tracker node
00115       if (!nodeHandle_.getParam ("camera_prefix", cameraPrefix) && !ros::param::get ("~camera_prefix", cameraPrefix))
00116           {
00117             ROS_WARN
00118               ("the camera_prefix parameter does not exist.\n"
00119                "This may mean that:\n"
00120                "- the tracker is not launched,\n"
00121                "- the tracker and viewer are not running in the same namespace."
00122                );
00123           }
00124         else if (cameraPrefix.empty ())
00125           {
00126             ROS_INFO
00127               ("tracker is not yet initialized, waiting...\n"
00128                "You may want to launch the client to initialize the tracker.");
00129           }
00130         if (this->exiting())
00131           return;
00132         rate.sleep ();
00133       }
00134     nodeHandlePrivate_.param<double>("frame_size", frameSize_, 0.1);
00135 
00136     rectifiedImageTopic_ =
00137       ros::names::resolve(cameraPrefix + "/image_rect");
00138     cameraInfoTopic_ =
00139       ros::names::resolve(cameraPrefix + "/camera_info");
00140 
00141     initCallback_t initCallback =
00142       boost::bind(&TrackerViewer::initCallback, this, _1, _2);
00143 
00144     reconfigureCallback_t reconfigureCallback =
00145       boost::bind(&TrackerViewer::reconfigureCallback, this, _1, _2);
00146 
00147     initService_ = nodeHandle_.advertiseService
00148       (visp_tracker::init_service_viewer, initCallback);
00149 
00150     reconfigureService_ = nodeHandle_.advertiseService
00151         (visp_tracker::reconfigure_service_viewer, reconfigureCallback);
00152 
00153 
00154     boost::filesystem::ofstream modelStream;
00155     std::string path;
00156 
00157     unsigned int cpt = 0;
00158     while (!nodeHandle_.hasParam(visp_tracker::model_description_param))
00159     {
00160       if (!nodeHandle_.hasParam(visp_tracker::model_description_param))
00161       {
00162         if(cpt%10 == 0){
00163           ROS_WARN_STREAM
00164               ("[Node: " << ros::this_node::getName() << "]\n"
00165                "The model_description parameter does not exist.\n"
00166                "This may mean that:\n"
00167                "- the tracker is not launched or not initialized,\n"
00168                "- the tracker and viewer are not running in the same namespace."
00169                );
00170         }
00171         cpt++;
00172       }
00173       if (this->exiting())
00174         return;
00175       rate.sleep ();
00176     }
00177 
00178 
00179     if (!makeModelFile(modelStream, path))
00180       throw std::runtime_error
00181   ("failed to load the model from the parameter server");
00182 
00183     ROS_INFO_STREAM("Model loaded from the parameter server.");
00184     //ROS_WARN_STREAM("Make model Viewer: " << path.c_str());
00185     modelPath_ = path;
00186 
00187     initializeTracker();
00188 
00189     if (this->exiting())
00190       return;
00191 
00192     checkInputs();
00193     if (this->exiting())
00194       return;
00195 
00196     // Subscribe to camera and tracker synchronously.
00197     imageSubscriber_.subscribe
00198       (imageTransport_, rectifiedImageTopic_, queueSize_);
00199     cameraInfoSubscriber_.subscribe
00200       (nodeHandle_, cameraInfoTopic_, queueSize_);
00201     trackingResultSubscriber_.subscribe
00202       (nodeHandle_, visp_tracker::object_position_covariance_topic,
00203        queueSize_);
00204     movingEdgeSitesSubscriber_.subscribe
00205       (nodeHandle_, visp_tracker::moving_edge_sites_topic, queueSize_);
00206     kltPointsSubscriber_.subscribe
00207       (nodeHandle_, visp_tracker::klt_points_topic, queueSize_);
00208 
00209     synchronizer_.connectInput
00210       (imageSubscriber_, cameraInfoSubscriber_,
00211        trackingResultSubscriber_, movingEdgeSitesSubscriber_, kltPointsSubscriber_);
00212     synchronizer_.registerCallback(boost::bind(&TrackerViewer::callback,
00213                  this, _1, _2, _3, _4, _5));
00214 
00215     // Check for synchronization every 30s.
00216     synchronizer_.registerCallback(boost::bind(increment, &countAll_));
00217     imageSubscriber_.registerCallback(boost::bind(increment, &countImages_));
00218     cameraInfoSubscriber_.registerCallback
00219       (boost::bind(increment, &countCameraInfo_));
00220     trackingResultSubscriber_.registerCallback
00221       (boost::bind(increment, &countTrackingResult_));
00222     movingEdgeSitesSubscriber_.registerCallback
00223       (boost::bind(increment, &countMovingEdgeSites_));
00224     kltPointsSubscriber_.registerCallback
00225       (boost::bind(increment, &countKltPoints_));
00226 
00227     timer_ = nodeHandle_.createWallTimer
00228       (ros::WallDuration(30.),
00229        boost::bind(&TrackerViewer::timerCallback, this));
00230 
00231     // Wait for image.
00232     waitForImage();
00233     if (this->exiting())
00234       return;
00235     if (!image_.getWidth() || !image_.getHeight())
00236       throw std::runtime_error("failed to retrieve image");
00237 
00238     // Load camera parameters.
00239     initializeVpCameraFromCameraInfo(cameraParameters_, info_);
00240     tracker_.setCameraParameters(cameraParameters_);
00241 
00242     // Load the common parameters from ROS messages
00243     loadCommonParameters();
00244   }
00245 
00246   void
00247   TrackerViewer::spin()
00248   {
00249     boost::format fmtWindowTitle ("ViSP MBT tracker viewer - [ns: %s]");
00250     fmtWindowTitle % ros::this_node::getNamespace ();
00251 
00252     vpDisplayX d(image_,
00253                  image_.getWidth(), image_.getHeight(),
00254                  fmtWindowTitle.str().c_str());
00255     vpImagePoint point (10, 10);
00256     vpImagePoint pointTime (22, 10);
00257     vpImagePoint pointCameraTopic (34, 10);
00258     ros::Rate loop_rate(80);
00259 
00260     boost::format fmt("tracking (x=%f y=%f z=%f)");
00261     boost::format fmtTime("time = %f");
00262 
00263     boost::format fmtCameraTopic("camera topic = %s");
00264     fmtCameraTopic % rectifiedImageTopic_;
00265     while (!exiting())
00266     {
00267       vpDisplay::display(image_);
00268       displayMovingEdgeSites();
00269       displayKltPoints();
00270       if (cMo_)
00271       {
00272         try
00273         {
00274           tracker_.initFromPose(image_, *cMo_);
00275           tracker_.display(image_, *cMo_, cameraParameters_, vpColor::red);
00276           vpDisplay::displayFrame(image_, *cMo_, cameraParameters_, frameSize_, vpColor::none, 2);
00277         }
00278         catch(...)
00279         {
00280           ROS_DEBUG_STREAM_THROTTLE(10, "failed to display cmo");
00281         }
00282 
00283         ROS_DEBUG_STREAM_THROTTLE(10, "cMo viewer:\n" << *cMo_);
00284 
00285         fmt % (*cMo_)[0][3] % (*cMo_)[1][3] % (*cMo_)[2][3];
00286         vpDisplay::displayCharString
00287             (image_, point, fmt.str().c_str(), vpColor::red);
00288         fmtTime % info_->header.stamp.toSec();
00289         vpDisplay::displayCharString
00290             (image_, pointTime, fmtTime.str().c_str(), vpColor::red);
00291         vpDisplay::displayCharString
00292             (image_, pointCameraTopic, fmtCameraTopic.str().c_str(),
00293              vpColor::red);
00294       }
00295       else{
00296         vpDisplay::displayCharString
00297             (image_, point, "tracking failed", vpColor::red);
00298       }
00299 
00300       vpDisplay::flush(image_);
00301       ros::spinOnce();
00302       loop_rate.sleep();
00303     }
00304   }
00305 
00306   void
00307   TrackerViewer::waitForImage()
00308   {
00309     ros::Rate loop_rate(10);
00310     while (!exiting()
00311            && (!image_.getWidth() || !image_.getHeight()))
00312       {
00313     ROS_INFO_THROTTLE(1, "waiting for a rectified image...");
00314         ros::spinOnce();
00315         loop_rate.sleep();
00316       }
00317   }
00318 
00319   void
00320   TrackerViewer::checkInputs()
00321   {
00322     ros::V_string topics;
00323     topics.push_back(rectifiedImageTopic_);
00324     topics.push_back(cameraInfoTopic_);
00325     topics.push_back(visp_tracker::object_position_topic);
00326     topics.push_back(visp_tracker::moving_edge_sites_topic);
00327     topics.push_back(visp_tracker::klt_points_topic);
00328     checkInputs_.start(topics, 60.0);
00329   }
00330 
00331   void
00332   TrackerViewer::loadCommonParameters()
00333   {
00334     nodeHandlePrivate_.param<std::string>("tracker_name", trackerName_, "");
00335     std::string key;
00336 
00337     bool loadParam = false;
00338 
00339     if(trackerName_.empty())
00340     {
00341       if(!ros::param::search("/angle_appear",key)){
00342         trackerName_ = "tracker_mbt";
00343         if(!ros::param::search(trackerName_ + "/angle_appear",key))
00344         {
00345           ROS_WARN_STREAM("No tracker has been found with the default name value \""
00346                           << trackerName_ << "/angle_appear\".\n"
00347                           << "Tracker name parameter (tracker_name) should be provided for this node (tracker_viewer).\n"
00348                           << "Polygon visibility might not work well in the viewer window.");
00349         }
00350         else loadParam = true;
00351       }
00352       else loadParam = true;
00353     }
00354     else loadParam = true;
00355 
00356     // Reading common parameters
00357     if(loadParam)
00358     {
00359       if (ros::param::search(trackerName_ + "/angle_appear",key))
00360       {
00361         double value;
00362         if(ros::param::get(key,value)){
00363           // ROS_WARN_STREAM("Angle Appear Viewer: " << value);
00364           tracker_.setAngleAppear(vpMath::rad(value));
00365         }
00366       }
00367       else
00368       {
00369         ROS_WARN_STREAM("No tracker has been found with the provided parameter "
00370                         << "(tracker_name=\"" << trackerName_ << "\")\n"
00371                         << "Polygon visibility might not work well in the viewer window");
00372       }
00373 
00374       if (ros::param::search(trackerName_ + "/angle_disappear",key))
00375       {
00376         double value;
00377         if(ros::param::get(key,value)){
00378           // ROS_WARN_STREAM("Angle Disappear Viewer: " << value);
00379           tracker_.setAngleDisappear(vpMath::rad(value));
00380         }
00381       }
00382     }
00383   }
00384 
00385   void
00386   TrackerViewer::initializeTracker()
00387   {
00388     try
00389       {
00390     // ROS_WARN_STREAM("Trying to load the model Viewer: " << modelPath_);
00391     tracker_.loadModel(modelPath_.native().c_str());
00392       }
00393     catch(...)
00394       {
00395         boost::format fmt("failed to load the model %1%");
00396     fmt % modelPath_;
00397         throw std::runtime_error(fmt.str());
00398       }
00399     // ROS_WARN("Model has been successfully loaded.");
00400   }
00401 
00402   void
00403   TrackerViewer::callback
00404   (const sensor_msgs::ImageConstPtr& image,
00405    const sensor_msgs::CameraInfoConstPtr& info,
00406    const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult,
00407    const visp_tracker::MovingEdgeSites::ConstPtr& sites,
00408    const visp_tracker::KltPoints::ConstPtr& klt)
00409   {
00410     // Copy image.
00411     try
00412       {
00413         rosImageToVisp(image_, image);
00414       }
00415     catch(std::exception& e)
00416       {
00417         ROS_ERROR_STREAM("dropping frame: " << e.what());
00418       }
00419 
00420     // Copy moving camera infos, edges sites and optional KLT points.
00421     info_ = info;
00422     sites_ = sites;
00423     klt_ = klt;
00424 
00425     // Copy cMo.
00426     cMo_ = vpHomogeneousMatrix();
00427     transformToVpHomogeneousMatrix(*cMo_, trackingResult->pose.pose);
00428   }
00429 
00430   void
00431   TrackerViewer::displayMovingEdgeSites()
00432   {
00433     if (!sites_)
00434       return;
00435     for (unsigned i = 0; i < sites_->moving_edge_sites.size(); ++i)
00436       {
00437         double x = sites_->moving_edge_sites[i].x;
00438         double y = sites_->moving_edge_sites[i].y;
00439         int suppress = sites_->moving_edge_sites[i].suppress;
00440         vpColor color = vpColor::black;
00441 
00442         switch(suppress)
00443           {
00444     case vpMeSite::NO_SUPPRESSION:
00445             color = vpColor::green;
00446             break;
00447     case vpMeSite::CONSTRAST:
00448             color = vpColor::blue;
00449             break;
00450     case vpMeSite::THRESHOLD:
00451             color = vpColor::purple;
00452             break;
00453     case vpMeSite::M_ESTIMATOR:
00454             color = vpColor::red;
00455             break;
00456     default: // vpMeSite::UNKOWN
00457       color = vpColor::yellow;
00458   }
00459 
00460         vpDisplay::displayCross(image_, vpImagePoint(x, y), 3, color, 1);
00461       }
00462   }
00463 
00464 
00465   void
00466   TrackerViewer::displayKltPoints()
00467   {
00468     if (!klt_)
00469       return;
00470     vpImagePoint pos;
00471 
00472     for (unsigned i = 0; i < klt_->klt_points_positions.size(); ++i)
00473     {
00474       double ii = klt_->klt_points_positions[i].i;
00475       double jj = klt_->klt_points_positions[i].j;
00476       int id = klt_->klt_points_positions[i].id;
00477       vpColor color = vpColor::red;
00478 
00479       vpDisplay::displayCross(image_, vpImagePoint(ii, jj), 15, color, 1);
00480 
00481       pos.set_i( vpMath::round( ii + 7 ) );
00482       pos.set_j( vpMath::round( jj + 7 ) );
00483       char ide[10];
00484       sprintf(ide, "%d", id);
00485       vpDisplay::displayCharString(image_, pos, ide, vpColor::red);
00486     }
00487   }
00488 
00489   void
00490   TrackerViewer::timerCallback()
00491   {
00492     if (countTrackingResult_ != countMovingEdgeSites_
00493         || countKltPoints_ != countMovingEdgeSites_)
00494     {
00495       boost::format fmt
00496           ("[visp_tracker] Low number of synchronized tuples received.\n"
00497            "Images: %d\n"
00498            "Camera info: %d\n"
00499            "Tracking result: %d\n"
00500            "Moving edge sites: %d\n"
00501            "KLT points: %d\n"
00502            "Synchronized tuples: %d\n"
00503            "Possible issues:\n"
00504            "\t* The network is too slow.");
00505       fmt % countImages_ % countCameraInfo_
00506           % countTrackingResult_ % countMovingEdgeSites_ % countKltPoints_ % countAll_;
00507       ROS_WARN_STREAM_THROTTLE(10, fmt.str());
00508     }
00509   }
00510 } // end of namespace visp_tracker.


visp_tracker
Author(s): Thomas Moulard
autogenerated on Sun Feb 19 2017 03:28:46