tracker-client.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <fstream>
00003 #include <sstream>
00004 #include <stdexcept>
00005 
00006 #include <boost/filesystem.hpp>
00007 #include <boost/filesystem/fstream.hpp>
00008 #include <boost/format.hpp>
00009 #include <boost/optional.hpp>
00010 #include <boost/version.hpp>
00011 
00012 #include <ros/ros.h>
00013 #include <ros/param.h>
00014 #include <ros/package.h>
00015 #include <dynamic_reconfigure/server.h>
00016 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00017 #include <image_proc/advertisement_checker.h>
00018 #include <image_transport/image_transport.h>
00019 #include <visp_tracker/Init.h>
00020 #include <visp_tracker/ModelBasedSettingsConfig.h>
00021 
00022 #include <visp/vpMe.h>
00023 #include <visp/vpPixelMeterConversion.h>
00024 #include <visp/vpPose.h>
00025 
00026 #define protected public
00027 #include <visp/vpMbEdgeTracker.h>
00028 #include <visp/vpMbKltTracker.h>
00029 #include <visp/vpMbEdgeKltTracker.h>
00030 #undef protected
00031 
00032 #include <visp/vpDisplayX.h>
00033 #include <visp/vpImageIo.h>
00034 #include <visp/vpIoTools.h>
00035 
00036 #include "conversion.hh"
00037 #include "callbacks.hh"
00038 #include "file.hh"
00039 #include "names.hh"
00040 
00041 #include "tracker-client.hh"
00042 
00043 
00044 namespace visp_tracker
00045 {
00046   TrackerClient::TrackerClient(ros::NodeHandle& nh,
00047                                ros::NodeHandle& privateNh,
00048                                volatile bool& exiting,
00049                                unsigned queueSize)
00050     : exiting_ (exiting),
00051       queueSize_(queueSize),
00052       nodeHandle_(nh),
00053       nodeHandlePrivate_(privateNh),
00054       imageTransport_(nodeHandle_),
00055       image_(),
00056       modelPath_(),
00057       modelPathAndExt_(),
00058       modelName_(),
00059       cameraPrefix_(),
00060       rectifiedImageTopic_(),
00061       cameraInfoTopic_(),
00062       trackerType_("mbt"),
00063       frameSize_(0.1),
00064       bModelPath_(),
00065       bInitPath_(),
00066       cameraSubscriber_(),
00067       mutex_(),
00068       reconfigureSrv_(NULL),
00069       reconfigureKltSrv_(NULL),
00070       reconfigureEdgeSrv_(NULL),
00071       movingEdge_(),
00072       kltTracker_(),
00073       cameraParameters_(),
00074       tracker_(),
00075       startFromSavedPose_(),
00076       checkInputs_(),
00077       resourceRetriever_()
00078   {
00079     // Parameters.
00080     nodeHandlePrivate_.param<std::string>("model_path", modelPath_,
00081                                           visp_tracker::default_model_path);
00082     nodeHandlePrivate_.param<std::string>("model_name", modelName_, "");
00083 
00084     nodeHandlePrivate_.param<bool>
00085       ("start_from_saved_pose", startFromSavedPose_, false);
00086 
00087     nodeHandlePrivate_.param<bool>
00088       ("confirm_init", confirmInit_, true);
00089 
00090     nodeHandlePrivate_.param<std::string>("tracker_type", trackerType_, "mbt");
00091     if(trackerType_=="mbt")
00092       tracker_ = new vpMbEdgeTracker();
00093     else if(trackerType_=="klt")
00094       tracker_ = new vpMbKltTracker();
00095     else
00096       tracker_ = new vpMbEdgeKltTracker();
00097 
00098     nodeHandlePrivate_.param<double>("frame_size", frameSize_, 0.1);
00099 
00100     //tracker_->resetTracker(); // TO CHECK
00101 
00102     if (modelName_.empty ())
00103       throw std::runtime_error
00104         ("empty model\n"
00105          "Relaunch the client while setting the model_name parameter, i.e.\n"
00106          "$ rosrun visp_tracker client _model_name:=my-model"
00107          );
00108 
00109     // Compute topic and services names.
00110 
00111     ros::Rate rate (1);
00112     while (cameraPrefix_.empty ())
00113       {
00114       if (!nodeHandle_.getParam ("camera_prefix", cameraPrefix_) && !ros::param::get ("~camera_prefix", cameraPrefix_))
00115     {
00116             ROS_WARN
00117               ("the camera_prefix parameter does not exist.\n"
00118                "This may mean that:\n"
00119                "- the tracker is not launched,\n"
00120                "- the tracker and viewer are not running in the same namespace."
00121                );
00122           }
00123         else if (cameraPrefix_.empty ())
00124           {
00125             ROS_INFO
00126               ("tracker is not yet initialized, waiting...\n"
00127                "You may want to launch the client to initialize the tracker.");
00128           }
00129         if (this->exiting())
00130           return;
00131         rate.sleep ();
00132       }
00133 
00134     rectifiedImageTopic_ =
00135       ros::names::resolve(cameraPrefix_ + "/image_rect");
00136     cameraInfoTopic_ =
00137       ros::names::resolve(cameraPrefix_ + "/camera_info");
00138 
00139     // Check for subscribed topics.
00140     checkInputs();
00141 
00142     // Camera subscriber.
00143     cameraSubscriber_ = imageTransport_.subscribeCamera
00144       (rectifiedImageTopic_, queueSize_,
00145        bindImageCallback(image_, header_, info_));
00146 
00147     // Model loading.
00148     bModelPath_ = getModelFileFromModelName(modelName_, modelPath_);
00149     bInitPath_ = getInitFileFromModelName(modelName_, modelPath_);
00150 
00151     ROS_INFO_STREAM("Model file: " << bModelPath_);
00152     ROS_INFO_STREAM("Init file: " << bInitPath_);
00153 
00154     // Load the 3d model.
00155     loadModel();
00156 
00157     // Set callback for dynamic reconfigure.
00158     // No more necessary as it is done via the reconfigure server
00159 //    if(trackerType_!="klt"){
00160 //      vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker_);
00161 //      t->setMovingEdge(movingEdge_);
00162 //    }
00163 
00164 //    if(trackerType_!="mbt"){
00165 //      vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker_);
00166 //      t->setKltOpencv(kltTracker_);
00167 //    }
00168 
00169     // Dynamic reconfigure.
00170     if(trackerType_=="mbt+klt"){ // Hybrid Tracker reconfigure
00171       reconfigureSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
00172       reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t::CallbackType f =
00173         boost::bind(&reconfigureCallback, boost::ref(tracker_),
00174                     boost::ref(image_), boost::ref(movingEdge_), boost::ref(kltTracker_),
00175                     boost::ref(mutex_), _1, _2);
00176       reconfigureSrv_->setCallback(f);
00177     }
00178     else if(trackerType_=="mbt"){ // Edge Tracker reconfigure
00179       reconfigureEdgeSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
00180       reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t::CallbackType f =
00181         boost::bind(&reconfigureEdgeCallback, boost::ref(tracker_),
00182                     boost::ref(image_), boost::ref(movingEdge_),
00183                     boost::ref(mutex_), _1, _2);
00184       reconfigureEdgeSrv_->setCallback(f);
00185     }
00186     else{ // KLT Tracker reconfigure
00187       reconfigureKltSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
00188       reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t::CallbackType f =
00189         boost::bind(&reconfigureKltCallback, boost::ref(tracker_),
00190                     boost::ref(image_), boost::ref(kltTracker_),
00191                     boost::ref(mutex_), _1, _2);
00192       reconfigureKltSrv_->setCallback(f);
00193     }
00194 
00195     // Wait for the image to be initialized.
00196     waitForImage();
00197     if (this->exiting())
00198       return;
00199     if (!image_.getWidth() || !image_.getHeight())
00200       throw std::runtime_error("failed to retrieve image");
00201 
00202     // Tracker initialization.
00203     // - Camera
00204     initializeVpCameraFromCameraInfo(cameraParameters_, info_);
00205     tracker_->setCameraParameters(cameraParameters_);
00206     tracker_->setDisplayFeatures(true);
00207 
00208     ROS_INFO_STREAM(convertVpMbTrackerToRosMessage(tracker_));
00209     // - Moving edges.
00210     if(trackerType_!="klt"){
00211       // No more necessary as it has been done via the reconfigure server
00212 //      movingEdge_.initMask();
00213 //      vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker_);
00214 //      t->setMovingEdge(movingEdge_);
00215       ROS_INFO_STREAM(convertVpMeToRosMessage(tracker_, movingEdge_));
00216       //movingEdge_.print();
00217     }
00218     
00219     if(trackerType_!="mbt"){
00220       // No more necessary as it has been done via the reconfigure server
00221 //      vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker_);
00222 //      t->setKltOpencv(kltTracker_);
00223       ROS_INFO_STREAM(convertVpKltOpencvToRosMessage(tracker_,kltTracker_));
00224     }
00225 
00226     // Display camera parameters and moving edges settings.
00227     ROS_INFO_STREAM(cameraParameters_);
00228   }
00229 
00230   void
00231   TrackerClient::checkInputs()
00232   {
00233     ros::V_string topics;
00234     topics.push_back(rectifiedImageTopic_);
00235     topics.push_back(cameraInfoTopic_);
00236     checkInputs_.start(topics, 60.0);
00237   }
00238 
00239   void
00240   TrackerClient::spin()
00241   {
00242     boost::format fmtWindowTitle ("ViSP MBT tracker initialization - [ns: %s]");
00243     fmtWindowTitle % ros::this_node::getNamespace ();
00244 
00245     vpDisplayX d(image_, image_.getWidth(), image_.getHeight(),
00246                  fmtWindowTitle.str().c_str());
00247 
00248     ros::Rate loop_rate_tracking(200);
00249     bool ok = false;
00250     vpHomogeneousMatrix cMo;
00251     vpImagePoint point (10, 10);
00252     while (!ok && !exiting())
00253     {
00254       try
00255       {
00256         // Initialize.
00257         vpDisplay::display(image_);
00258         vpDisplay::flush(image_);
00259         if (!startFromSavedPose_)
00260           init();
00261         else
00262         {
00263           cMo = loadInitialPose();
00264           startFromSavedPose_ = false;
00265           tracker_->initFromPose(image_, cMo);
00266         }
00267         tracker_->getPose(cMo);
00268 
00269         ROS_INFO_STREAM("initial pose [tx,ty,tz,tux,tuy,tuz]:\n" << vpPoseVector(cMo).t());
00270 
00271         // Track once to make sure initialization is correct.
00272         if (confirmInit_)
00273         {
00274           vpImagePoint ip;
00275           vpMouseButton::vpMouseButtonType button =
00276               vpMouseButton::button1;
00277           do
00278           {
00279             vpDisplay::display(image_);
00280             mutex_.lock ();
00281             tracker_->track(image_);
00282             tracker_->getPose(cMo);
00283             tracker_->display(image_, cMo, cameraParameters_,
00284                               vpColor::red, 2);
00285             vpDisplay::displayFrame(image_, cMo, cameraParameters_,frameSize_,vpColor::none,2);
00286             mutex_.unlock();
00287             vpDisplay::displayCharString
00288                 (image_, point, "tracking, click to initialize tracker",
00289                  vpColor::red);
00290             vpDisplay::flush(image_);
00291 
00292             ros::spinOnce();
00293             loop_rate_tracking.sleep();
00294             if (exiting())
00295               return;
00296           }
00297           while(!vpDisplay::getClick(image_, ip, button, false));
00298           ok = true;
00299         }
00300         else
00301           ok = true;
00302       }
00303       catch(const std::runtime_error& e)
00304       {
00305         mutex_.unlock();
00306         ROS_ERROR_STREAM("failed to initialize: "
00307                          << e.what() << ", retrying...");
00308       }
00309       catch(const std::string& str)
00310       {
00311         mutex_.unlock();
00312         ROS_ERROR_STREAM("failed to initialize: "
00313                          << str << ", retrying...");
00314       }
00315       catch(...)
00316       {
00317         mutex_.unlock();
00318         ROS_ERROR("failed to initialize, retrying...");
00319       }
00320     }
00321 
00322     ROS_INFO_STREAM("Initialization done, sending initial cMo:\n" << cMo);
00323     try
00324     {
00325       sendcMo(cMo);
00326     }
00327     catch(std::exception& e)
00328     {
00329       ROS_ERROR_STREAM("failed to send cMo: " << e.what ());
00330     }
00331     catch(...)
00332     {
00333       ROS_ERROR("unknown error happened while sending the cMo, retrying...");
00334     }
00335   }
00336   
00337   TrackerClient::~TrackerClient()
00338   {
00339     delete tracker_;
00340 
00341     if(reconfigureSrv_ != NULL)
00342       delete reconfigureSrv_;
00343 
00344     if(reconfigureKltSrv_ != NULL)
00345       delete reconfigureKltSrv_;
00346 
00347     if(reconfigureEdgeSrv_ != NULL)
00348       delete reconfigureEdgeSrv_;
00349   }
00350 
00351   void
00352   TrackerClient::sendcMo(const vpHomogeneousMatrix& cMo)
00353   {
00354     ros::ServiceClient client =
00355         nodeHandle_.serviceClient<visp_tracker::Init>(visp_tracker::init_service);
00356 
00357 
00358     ros::ServiceClient clientViewer =
00359         nodeHandle_.serviceClient<visp_tracker::Init>(visp_tracker::init_service_viewer);
00360     visp_tracker::Init srv;
00361 
00362     // Load the model and send it to the parameter server.
00363     std::string modelDescription = fetchResource(modelPathAndExt_);
00364     nodeHandle_.setParam (model_description_param, modelDescription);
00365 
00366     vpHomogeneousMatrixToTransform(srv.request.initial_cMo, cMo);
00367     
00368     convertVpMbTrackerToInitRequest(tracker_, srv);
00369 
00370     if(trackerType_!="klt"){
00371       convertVpMeToInitRequest(movingEdge_, tracker_, srv);
00372     }
00373     
00374     if(trackerType_!="mbt"){
00375       convertVpKltOpencvToInitRequest(kltTracker_, tracker_, srv);
00376     }
00377 
00378     ros::Rate rate (1);
00379     while (!client.waitForExistence ())
00380     {
00381       ROS_INFO
00382           ("Waiting for the initialization service to become available.");
00383       rate.sleep ();
00384     }
00385 
00386     if (client.call(srv))
00387     {
00388       if (srv.response.initialization_succeed)
00389         ROS_INFO("Tracker initialized with success.");
00390       else
00391         throw std::runtime_error("failed to initialize tracker.");
00392     }
00393     else
00394       throw std::runtime_error("failed to call service init");
00395 
00396     if (clientViewer.call(srv))
00397     {
00398       if (srv.response.initialization_succeed)
00399         ROS_INFO("Tracker Viewer initialized with success.");
00400       else
00401         throw std::runtime_error("failed to initialize tracker viewer.");
00402     }
00403     else
00404        ROS_INFO("No Tracker Viewer node to initialize from service");
00405   }
00406 
00407   void
00408   TrackerClient::loadModel()
00409   {
00410     try
00411       {
00412         ROS_DEBUG_STREAM("Trying to load the model "
00413        << bModelPath_.native());
00414 
00415         std::string modelPath;
00416         boost::filesystem::ofstream modelStream;
00417         if (!makeModelFile(modelStream,
00418          bModelPath_.native(),
00419                            modelPath))
00420           throw std::runtime_error ("failed to retrieve model");
00421 
00422     tracker_->loadModel(modelPath);
00423   ROS_INFO("Model has been successfully loaded.");
00424 
00425   if(trackerType_=="mbt"){
00426     vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker_);
00427     ROS_DEBUG_STREAM("Nb faces: "
00428                      << t->getFaces().getPolygon().size());
00429     ROS_DEBUG_STREAM("Nb visible faces: " << t->getFaces().getNbVisiblePolygon());
00430 
00431     std::list<vpMbtDistanceLine *> linesList;
00432     t->getLline(linesList);
00433     ROS_DEBUG_STREAM("Nb line: " << linesList.size());
00434     ROS_DEBUG_STREAM("nline: " << t->nline);
00435   }
00436   else if(trackerType_=="klt"){
00437     vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker_);
00438     ROS_DEBUG_STREAM("Nb faces: "
00439                      << t->getFaces().getPolygon().size());
00440     ROS_DEBUG_STREAM("Nb visible faces: " << t->getFaces().getNbVisiblePolygon());
00441     ROS_DEBUG_STREAM("Nb KLT points: " << t->getNbKltPoints());
00442   }
00443   else {
00444     vpMbEdgeKltTracker* t = dynamic_cast<vpMbEdgeKltTracker*>(tracker_);
00445     ROS_DEBUG_STREAM("Nb hidden faces: "
00446                      << t->getFaces().getPolygon().size());
00447     ROS_DEBUG_STREAM("Nb visible faces: " << t->getFaces().getNbVisiblePolygon());
00448     ROS_DEBUG_STREAM("Nb KLT points: " << t->getNbKltPoints());
00449 
00450     std::list<vpMbtDistanceLine *> linesList;
00451     t->getLline(linesList);
00452     ROS_DEBUG_STREAM("Nb line: " << linesList.size());
00453     ROS_DEBUG_STREAM("nline: " << t->nline);
00454   }
00455       }
00456     catch(...)
00457       {
00458         boost::format fmt
00459           ("Failed to load the model %1%\n"
00460            "Do you use resource_retriever syntax?\n"
00461            "I.e. replace /my/model/path by file:///my/model/path");
00462   fmt % bModelPath_;
00463         throw std::runtime_error(fmt.str());
00464       }
00465   }
00466 
00467   vpHomogeneousMatrix
00468   TrackerClient::loadInitialPose()
00469   {
00470     vpHomogeneousMatrix cMo;
00471     cMo.eye();
00472 
00473     std::string initialPose = getInitialPoseFileFromModelName (modelName_, modelPath_);
00474     std::string resource;
00475     try
00476     {
00477       resource = fetchResource (initialPose);
00478       std::stringstream file;
00479       file << resource;
00480 
00481       if (!file.good())
00482       {
00483         ROS_WARN_STREAM("failed to load initial pose: " << initialPose << "\n"
00484                         << "using identity as initial pose");
00485         return cMo;
00486       }
00487 
00488       vpPoseVector pose;
00489       for (unsigned i = 0; i < 6; ++i) {
00490         if (file.good())
00491           file >> pose[i];
00492         else
00493         {
00494           ROS_WARN("failed to parse initial pose file");
00495           return cMo;
00496         }
00497       }
00498       cMo.buildFrom(pose);
00499       return cMo;
00500     }
00501     catch (...)
00502     {
00503       // Failed to retrieve initial pose since model path starts with http://, package://, file:///
00504       // We try to read from temporary file /tmp/$USER/
00505       std::string username;
00506       vpIoTools::getUserName(username);
00507 
00508       std::string filename;
00509   #if defined(_WIN32)
00510       filename ="C:/temp/" + username;
00511   #else
00512       filename ="/tmp/" + username;
00513   #endif
00514       filename += "/" + modelName_ + ".0.pos";
00515       ROS_INFO_STREAM("Try to read init pose from: " << filename);
00516       if (vpIoTools::checkFilename(filename)) {
00517         ROS_INFO_STREAM("Retrieve initial pose from: " << filename);
00518         std::ifstream in( filename.c_str() );
00519         vpPoseVector pose;
00520         pose.load(in);
00521         cMo.buildFrom(pose);
00522         in.close();
00523       }
00524 
00525       return cMo;
00526     }
00527   }
00528 
00529   void
00530   TrackerClient::saveInitialPose(const vpHomogeneousMatrix& cMo)
00531   {
00532     boost::filesystem::path initialPose = getInitialPoseFileFromModelName(modelName_, modelPath_);
00533     boost::filesystem::ofstream file(initialPose);
00534     if (!file.good())
00535     {
00536       // Failed to save initial pose since model path starts with http://, package://, file:///
00537       // We create a temporary file in /tmp/$USER/
00538       std::string username;
00539       vpIoTools::getUserName(username);
00540 
00541       // Create a log filename to save velocities...
00542       std::string logdirname;
00543   #if defined(_WIN32)
00544       logdirname ="C:/temp/" + username;
00545   #else
00546       logdirname ="/tmp/" + username;
00547   #endif
00548       // Test if the output path exist. If no try to create it
00549       if (vpIoTools::checkDirectory(logdirname) == false) {
00550         try {
00551           vpIoTools::makeDirectory(logdirname);
00552         }
00553         catch (...) {
00554           ROS_WARN_STREAM("Unable to create the folder " << logdirname << " to save the initial pose");
00555           return;
00556         }
00557       }
00558       std::string filename = logdirname + "/" + modelName_ + ".0.pos";
00559       ROS_INFO_STREAM("Save initial pose in: " << filename);
00560       std::fstream finitpos ;
00561       finitpos.open(filename.c_str(), std::ios::out) ;
00562       vpPoseVector pose;
00563       pose.buildFrom(cMo);
00564 
00565       finitpos << pose;
00566       finitpos.close();
00567     }
00568     else {
00569       ROS_INFO_STREAM("Save initial pose in: " << initialPose);
00570       vpPoseVector pose;
00571       pose.buildFrom(cMo);
00572       file << pose;
00573     }
00574   }
00575 
00576   TrackerClient::points_t
00577   TrackerClient::loadInitializationPoints()
00578   {
00579     points_t points;
00580 
00581     std::string init =
00582         getInitFileFromModelName(modelName_, modelPath_);
00583     std::string resource = fetchResource(init);
00584     std::stringstream file;
00585     file << resource;
00586 
00587     if (!file.good())
00588     {
00589       boost::format fmt("failed to load initialization points: %1");
00590       fmt % init;
00591       throw std::runtime_error(fmt.str());
00592     }
00593 
00594     char c;
00595     // skip lines starting with # as comment
00596     file.get(c);
00597     while (!file.fail() && (c == '#')) {
00598       file.ignore(256, '\n');
00599       file.get(c);
00600     }
00601     file.unget();
00602 
00603     unsigned int npoints;
00604     file >> npoints;
00605     file.ignore(256, '\n'); // skip the rest of the line
00606     ROS_INFO_STREAM("Number of 3D points  " << npoints << "\n");
00607 
00608     if (npoints > 100000) {
00609       throw vpException(vpException::badValue,
00610                         "Exceed the max number of points.");
00611     }
00612 
00613     vpPoint point;
00614     double X = 0., Y = 0., Z = 0.;
00615     for (unsigned int i=0 ; i < npoints ; i++){
00616       // skip lines starting with # as comment
00617       file.get(c);
00618       while (!file.fail() && (c == '#')) {
00619         file.ignore(256, '\n');
00620         file.get(c);
00621       }
00622       file.unget();
00623 
00624       file >> X >> Y >> Z ;
00625       file.ignore(256, '\n'); // skip the rest of the line
00626 
00627       point.setWorldCoordinates(X,Y,Z) ; // (X,Y,Z)
00628       points.push_back(point);
00629     }
00630 
00631     return points;
00632   }
00633 
00634   bool
00635   TrackerClient::validatePose(const vpHomogeneousMatrix &cMo){
00636     ros::Rate loop_rate(200);
00637     vpImagePoint ip;
00638     vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
00639     vpDisplay::display(image_);
00640     tracker_->setDisplayFeatures(false);
00641     tracker_->display(image_, cMo, cameraParameters_, vpColor::green);
00642     vpDisplay::displayFrame(image_, cMo, cameraParameters_,frameSize_,vpColor::none,2);
00643     vpDisplay::displayCharString(image_, 15, 10,
00644         "Left click to validate, right click to modify initial pose",
00645         vpColor::red);
00646     vpDisplay::flush(image_);
00647     tracker_->setDisplayFeatures(true);
00648 
00649     do
00650     {
00651       ros::spinOnce();
00652       loop_rate.sleep();
00653       if (!ros::ok())
00654         return false;
00655     }
00656     while(ros::ok() && !vpDisplay::getClick(image_, ip, button, false));
00657 
00658     if(button == vpMouseButton::button1)
00659       return true;
00660 
00661     return false;
00662   }
00663 
00664   void
00665   TrackerClient::init()
00666   {
00667     ros::Rate loop_rate(200);
00668     vpHomogeneousMatrix cMo;
00669     vpImagePoint point (10, 10);
00670 
00671     cMo = loadInitialPose();
00672     tracker_->initFromPose(image_, cMo);
00673 
00674     // Show last cMo.
00675     vpImagePoint ip;
00676     vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
00677 
00678     if(validatePose(cMo))
00679     {
00680       return;
00681     }
00682     vpDisplayX *initHelpDisplay = NULL;
00683 
00684     std::string helpImagePath;
00685     nodeHandlePrivate_.param<std::string>("help_image_path", helpImagePath, "");
00686     if (helpImagePath.empty()){
00687 
00688       resource_retriever::MemoryResource resource;
00689 
00690       try{
00691         resource = resourceRetriever_.get( getHelpImageFileFromModelName(modelName_, modelPath_) );
00692         char* tmpname = strdup("/tmp/tmpXXXXXX");
00693         if (mkdtemp(tmpname) == NULL) {
00694           ROS_ERROR_STREAM("Failed to create the temporary directory: " << strerror(errno));
00695         }
00696         else {
00697           boost::filesystem::path path(tmpname);
00698           path /= ("help.ppm");
00699           free(tmpname);
00700 
00701           helpImagePath = path.native();
00702           ROS_INFO("Copy help image from %s to %s", getHelpImageFileFromModelName(modelName_, modelPath_).c_str(),
00703                    helpImagePath.c_str());
00704 
00705 
00706           FILE* f = fopen(helpImagePath.c_str(), "w");
00707           fwrite(resource.data.get(), resource.size, 1, f);
00708           fclose(f);
00709         }
00710       }
00711       catch(...){
00712       }
00713 
00714       ROS_WARN_STREAM("Auto detection of help file: " << helpImagePath);
00715     }
00716 
00717     if (!helpImagePath.empty()){
00718       try {
00719         // check if the file exists
00720         if (! vpIoTools::checkFilename(helpImagePath)) {
00721           ROS_WARN("Error tracker initialization help image file \"%s\" doesn't exist", helpImagePath.c_str());
00722         }
00723         else {
00724           ROS_INFO_STREAM("Load help image: " << helpImagePath);
00725           int winx = 0;
00726           int winy = 0;
00727 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00728           winx = image_.display->getWindowXPosition();
00729           winy = image_.display->getWindowYPosition();
00730 #endif
00731           initHelpDisplay = new vpDisplayX (winx+image_.getWidth()+20, winy, "Init help image");
00732 
00733           vpImage<vpRGBa> initHelpImage;
00734           vpImageIo::read(initHelpImage, helpImagePath);
00735           initHelpDisplay->init(initHelpImage);
00736           vpDisplay::display(initHelpImage);
00737           vpDisplay::flush(initHelpImage);
00738         }
00739       } catch(vpException &e) {
00740         ROS_WARN("Error diplaying tracker initialization help image file \"%s\":\n%s", helpImagePath.c_str(), e.what());
00741       }
00742     }
00743 
00744     points_t points = loadInitializationPoints();
00745     imagePoints_t imagePoints;
00746 
00747     bool done = false;
00748     while(!done){
00749       vpDisplay::display(image_);
00750       vpDisplay::flush(image_);
00751 
00752       imagePoints.clear();
00753       for(unsigned i = 0; i < points.size(); ++i)
00754       {
00755         do
00756         {
00757           ros::spinOnce();
00758           loop_rate.sleep();
00759           if (!ros::ok())
00760             return;
00761         }
00762         while(ros::ok() && !vpDisplay::getClick(image_, ip, button, false));
00763 
00764         imagePoints.push_back(ip);
00765         vpDisplay::displayCross(image_, imagePoints.back(), 5, vpColor::green);
00766         vpDisplay::flush(image_);
00767       }
00768 
00769       tracker_->initFromPoints(image_,imagePoints,points);
00770       tracker_->getPose(cMo);
00771       if(validatePose(cMo))
00772         done = true;
00773     }
00774     tracker_->initFromPose(image_, cMo);
00775     saveInitialPose(cMo);
00776     if (initHelpDisplay != NULL)
00777       delete initHelpDisplay;
00778   }
00779 
00780   void
00781   TrackerClient::initPoint(unsigned& i,
00782                            points_t& points,
00783                            imagePoints_t& imagePoints,
00784                            ros::Rate& rate,
00785                            vpPose& pose)
00786   {
00787     vpImagePoint ip;
00788     double x = 0., y = 0.;
00789     boost::format fmt("click on point %u/%u");
00790     fmt % (i + 1) % points.size(); // list points from 1 to n.
00791 
00792     // Click on the point.
00793     vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
00794     do
00795       {
00796         vpDisplay::display(image_);
00797         vpDisplay::displayCharString
00798           (image_, 15, 10,
00799            fmt.str().c_str(),
00800            vpColor::red);
00801 
00802         for (unsigned j = 0; j < imagePoints.size(); ++j)
00803           vpDisplay::displayCross(image_, imagePoints[j], 5, vpColor::green);
00804 
00805         vpDisplay::flush(image_);
00806         ros::spinOnce();
00807         rate.sleep();
00808         if (exiting())
00809           return;
00810       }
00811     while(!vpDisplay::getClick(image_, ip, button, false));
00812 
00813     imagePoints.push_back(ip);
00814     vpPixelMeterConversion::convertPoint(cameraParameters_, ip, x, y);
00815     points[i].set_x(x);
00816     points[i].set_y(y);
00817     pose.addPoint(points[i]);
00818   }
00819 
00820   void
00821   TrackerClient::waitForImage()
00822   {
00823     ros::Rate loop_rate(10);
00824     while (!exiting()
00825            && (!image_.getWidth() || !image_.getHeight()))
00826       {
00827         ROS_INFO_THROTTLE(1, "waiting for a rectified image...");
00828         ros::spinOnce();
00829         loop_rate.sleep();
00830       }
00831   }
00832 
00833   std::string
00834   TrackerClient::fetchResource(const std::string& s)
00835   {
00836     resource_retriever::MemoryResource resource =
00837       resourceRetriever_.get(s);
00838     std::string result;
00839     result.resize(resource.size);
00840     unsigned i = 0;
00841     for (; i < resource.size; ++i)
00842       result[i] = resource.data.get()[i];
00843     return result;
00844   }
00845 
00846   bool
00847   TrackerClient::makeModelFile(boost::filesystem::ofstream& modelStream,
00848                                const std::string& resourcePath,
00849                                std::string& fullModelPath)
00850   {
00851     std::string modelExt_ = ".wrl";
00852     bool vrmlWorked = true;
00853     resource_retriever::MemoryResource resource;
00854 
00855     try{
00856       resource = resourceRetriever_.get(resourcePath + modelExt_);
00857     }
00858     catch(...){
00859       vrmlWorked = false;
00860     }
00861 
00862     if(!vrmlWorked){
00863       modelExt_ = ".cao";
00864 
00865       try{
00866         resource = resourceRetriever_.get(resourcePath + modelExt_);
00867       }
00868       catch(...){
00869         ROS_ERROR_STREAM("No .cao nor .wrl file found in: " << resourcePath);
00870       }
00871     }
00872 
00873     modelPathAndExt_ = resourcePath + modelExt_;
00874 
00875     //ROS_WARN_STREAM("Model file Make Client: " << resourcePath << modelExt_);
00876 
00877     // Crash after when model not found
00878     std::string result;
00879     result.resize(resource.size);
00880     unsigned i = 0;
00881     for (; i < resource.size; ++i)
00882       result[i] = resource.data.get()[i];
00883     result[resource.size];
00884 
00885     char* tmpname = strdup("/tmp/tmpXXXXXX");
00886     if (mkdtemp(tmpname) == NULL)
00887     {
00888       ROS_ERROR_STREAM
00889           ("Failed to create the temporary directory: " << strerror(errno));
00890       return false;
00891     }
00892     boost::filesystem::path path(tmpname);
00893     path /= ("model" + modelExt_);
00894     free(tmpname);
00895 
00896     fullModelPath = path.native();
00897 
00898 
00899     //ROS_WARN_STREAM("Model file Make Client Full path tmp: " << fullModelPath );
00900 
00901     modelStream.open(path);
00902     if (!modelStream.good())
00903     {
00904       ROS_ERROR_STREAM
00905           ("Failed to create the temporary file: " << path);
00906       return false;
00907     }
00908     modelStream << result;
00909     modelStream.flush();
00910     return true;
00911   }
00912 } // end of namespace visp_tracker.


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