node.cpp
Go to the documentation of this file.
00001 #include "node.h"
00002 #include "names.h"
00003 
00004 //command line parameters
00005 #include "cmd_line/cmd_line.h"
00006 
00007 
00008 //tracking
00009 #include "libauto_tracker/tracking.h"
00010 #include "libauto_tracker/threading.h"
00011 #include "libauto_tracker/events.h"
00012 
00013 #include "visp_tracker/MovingEdgeSites.h"
00014 #include "visp_tracker/KltPoints.h"
00015 
00016 //visp includes
00017 #include <visp/vpDisplayX.h>
00018 #include <visp/vpMbEdgeKltTracker.h>
00019 #include <visp/vpTime.h>
00020 
00021 //detectors
00022 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00023 #  include "detectors/datamatrix/detector.h"
00024 #  include "detectors/qrcode/detector.h"
00025 #else
00026 #  include <visp/vpDetectorDataMatrixCode.h>
00027 #  include <visp/vpDetectorQRCode.h>
00028 #endif
00029 
00030 #include <visp_bridge/camera.h>
00031 #include <visp_bridge/image.h>
00032 #include <visp_bridge/3dpose.h>
00033 
00034 #include "libauto_tracker/tracking.h"
00035 
00036 #include "resource_retriever/retriever.h"
00037 
00038 #include "std_msgs/Int8.h"
00039 #include "std_msgs/String.h"
00040 
00041 namespace visp_auto_tracker{
00042         Node::Node() :
00043                         n_("~"),
00044                         queue_size_(1),
00045                         tracker_config_path_(),
00046                         model_description_(),
00047                         model_path_(),
00048                         model_name_(),
00049                         code_message_(),
00050                         debug_display_(false),
00051                         I_(),
00052                         image_header_(),
00053                         got_image_(false),
00054                         cam_(),
00055                         t_(NULL) {
00056                 //get the tracker configuration file
00057                 //this file contains all of the tracker's parameters, they are not passed to ros directly.
00058                 n_.param<std::string>("tracker_config_path", tracker_config_path_, "");
00059                 n_.param<bool>("debug_display", debug_display_, false);
00060                 std::string model_full_path;
00061                 n_.param<std::string>("model_path", model_path_, "");
00062                 n_.param<std::string>("model_name", model_name_, "");
00063                 n_.param<std::string>("code_message", code_message_, "");
00064                 model_path_= model_path_[model_path_.length()-1]=='/'?model_path_:model_path_+std::string("/");
00065                 model_full_path = model_path_+model_name_;
00066                 tracker_config_path_ = model_full_path+".cfg";
00067                 ROS_INFO("model full path=%s",model_full_path.c_str());
00068 
00069                 //Parse command line arguments from config file (as ros param)
00070                 cmd_.init(tracker_config_path_);
00071                 cmd_.set_data_directory(model_path_); //force data path
00072                 cmd_.set_pattern_name(model_name_); //force model name
00073                 cmd_.set_show_fps(false);
00074                 if (! code_message_.empty()) {
00075                   ROS_WARN_STREAM("Track only code with message: \"" << code_message_ << "\"");
00076                   cmd_.set_code_message(code_message_);
00077                 }
00078 
00079                 resource_retriever::Retriever r;
00080                 resource_retriever::MemoryResource res;
00081                 try {
00082                   res = r.get(std::string("file://")+cmd_.get_mbt_cad_file());
00083                 }
00084                 catch(...) {
00085                   ROS_ERROR_STREAM("Unable to read wrl or cao model file as resource: " << std::string("file://")+cmd_.get_mbt_cad_file());
00086                 }
00087 
00088                 model_description_.resize(res.size);
00089                 for (unsigned int i=0; i < res.size; ++i)
00090                         model_description_[i] = res.data.get()[i];
00091 
00092                 ROS_INFO("Model content=%s",model_description_.c_str());
00093 
00094                 n_.setParam ("/model_description", model_description_);
00095         }
00096 
00097         void Node::waitForImage(){
00098             while ( ros::ok ()){
00099                 if(got_image_) return;
00100                 ros::spinOnce();
00101               }
00102         }
00103 
00104         //records last recieved image
00105         void Node::frameCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& cam_info){
00106                 boost::mutex::scoped_lock(lock_);
00107                 image_header_ = image->header;
00108                 I_ = visp_bridge::toVispImageRGBa(*image); //make sure the image isn't worked on by locking a mutex
00109                 cam_ = visp_bridge::toVispCameraParameters(*cam_info);
00110 
00111                 got_image_ = true;
00112         }
00113 
00114         void Node::spin(){
00115 
00116                 if(cmd_.should_exit()) return; //exit if needed
00117 
00118                 vpMbTracker* tracker; //mb-tracker will be chosen according to config
00119 
00120                 //create display
00121                 vpDisplayX* d = NULL;
00122                 if(debug_display_)
00123                   d = new vpDisplayX();
00124 
00125                 //init detector based on user preference
00126 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00127                 detectors::DetectorBase* detector = NULL;
00128                 if (cmd_.get_detector_type() == CmdLine::ZBAR)
00129                         detector = new detectors::qrcode::Detector;
00130                 else if(cmd_.get_detector_type() == CmdLine::DMTX)
00131                         detector = new detectors::datamatrix::Detector;
00132 #else // ViSP >= 2.10.0. In that case we use the detectors from ViSP
00133                 vpDetectorBase *detector = NULL;
00134                 if (cmd_.get_detector_type() == CmdLine::ZBAR)
00135                         detector = new vpDetectorQRCode;
00136                 else if(cmd_.get_detector_type() == CmdLine::DMTX)
00137                         detector = new vpDetectorDataMatrixCode;
00138 #endif
00139 
00140 #if 0
00141                 //init tracker based on user preference
00142                 if(cmd_.get_tracker_type() == CmdLine::KLT)
00143                         tracker = new vpMbKltTracker();
00144                 else if(cmd_.get_tracker_type() == CmdLine::KLT_MBT)
00145                         tracker = new vpMbEdgeKltTracker();
00146                 else if(cmd_.get_tracker_type() == CmdLine::MBT)
00147                         tracker = new vpMbEdgeTracker();
00148 #else
00149                 // Use the best tracker
00150                 tracker = new vpMbEdgeKltTracker();
00151 #endif
00152                 tracker->setCameraParameters(cam_);
00153                 tracker->setDisplayFeatures(true);
00154 
00155                 //compile detectors and paramters into the automatic tracker.
00156                 t_ = new tracking::Tracker(cmd_, detector, tracker, debug_display_);
00157                 t_->start(); //start the state machine
00158 
00159                 //subscribe to ros topics and prepare a publisher that will publish the pose
00160                 message_filters::Subscriber<sensor_msgs::Image> raw_image_subscriber(n_, image_topic, queue_size_);
00161                 message_filters::Subscriber<sensor_msgs::CameraInfo> camera_info_subscriber(n_, camera_info_topic, queue_size_);
00162                 message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> image_info_sync(raw_image_subscriber, camera_info_subscriber, queue_size_);
00163                 image_info_sync.registerCallback(boost::bind(&Node::frameCallback,this, _1, _2));
00164                 ros::Publisher object_pose_publisher = n_.advertise<geometry_msgs::PoseStamped>(object_position_topic, queue_size_);
00165                 ros::Publisher object_pose_covariance_publisher = n_.advertise<geometry_msgs::PoseWithCovarianceStamped>(object_position_covariance_topic, queue_size_);
00166                 ros::Publisher moving_edge_sites_publisher = n_.advertise<visp_tracker::MovingEdgeSites>(moving_edge_sites_topic, queue_size_);
00167                 ros::Publisher klt_points_publisher = n_.advertise<visp_tracker::KltPoints>(klt_points_topic, queue_size_);
00168                 ros::Publisher status_publisher = n_.advertise<std_msgs::Int8>(status_topic, queue_size_);
00169                 ros::Publisher code_message_publisher = n_.advertise<std_msgs::String>(code_message_topic, queue_size_);
00170 
00171                 //wait for an image to be ready
00172                 waitForImage();
00173                 {
00174                         //when an image is ready tell the tracker to start searching for patterns
00175                         boost::mutex::scoped_lock(lock_);
00176                         if(debug_display_) {
00177                           d->init(I_); //also init display
00178                           vpDisplay::setTitle(I_, "visp_auto_tracker debug display");
00179                         }
00180 
00181                         t_->process_event(tracking::select_input(I_));
00182                 }
00183 
00184                 unsigned int iter=0;
00185                 geometry_msgs::PoseStamped ps;
00186                 geometry_msgs::PoseWithCovarianceStamped ps_cov;
00187 
00188                 ros::Rate rate(30); //init 25fps publishing frequency
00189                 while(ros::ok()){
00190                   double t = vpTime::measureTimeMs();
00191                         boost::mutex::scoped_lock(lock_);
00192                         //process the new frame with the tracker
00193                         t_->process_event(tracking::input_ready(I_,cam_,iter));
00194                         //When the tracker is tracking, it's in the tracking::TrackModel state
00195                         //Access this state and broadcast the pose
00196                         tracking::TrackModel& track_model = t_->get_state<tracking::TrackModel&>();
00197 
00198                         ps.pose = visp_bridge::toGeometryMsgsPose(track_model.cMo); //convert
00199 
00200                         // Publish resulting pose.
00201                         if (object_pose_publisher.getNumSubscribers     () > 0)
00202                         {
00203                             ps.header = image_header_;
00204                             ps.header.frame_id = tracker_ref_frame;
00205                             object_pose_publisher.publish(ps);
00206                         }
00207 
00208                         // Publish resulting pose covariance matrix.
00209                         if (object_pose_covariance_publisher.getNumSubscribers  () > 0)
00210                         {
00211                             ps_cov.pose.pose = ps.pose;
00212                             ps_cov.header = image_header_;
00213                             ps_cov.header.frame_id = tracker_ref_frame;
00214 
00215                             for (unsigned i = 0; i < track_model.covariance.getRows(); ++i)
00216                             {
00217                                 for (unsigned j = 0; j < track_model.covariance.getCols(); ++j)
00218                                 {
00219                                     unsigned idx = i * track_model.covariance.getCols() + j;
00220                                     if (idx >= 36)
00221                                         continue;
00222                                     ps_cov.pose.covariance[idx] = track_model.covariance[i][j];
00223                                 }
00224                             }
00225 
00226                             object_pose_covariance_publisher.publish(ps_cov);
00227                         }
00228 
00229                         // Publish state machine status.
00230                         if (status_publisher.getNumSubscribers  () > 0)
00231                         {
00232                             std_msgs::Int8 status;
00233                             status.data = (unsigned char)(*(t_->current_state()));
00234                             status_publisher.publish(status);
00235                         }
00236 
00237                         // Publish moving edge sites.
00238                         if (moving_edge_sites_publisher.getNumSubscribers       () > 0)
00239                         {
00240                             visp_tracker::MovingEdgeSitesPtr sites (new visp_tracker::MovingEdgeSites);
00241                             // Test if we are in the state tracking::TrackModel. In that case the pose is good;
00242                             // we can send the moving edges. Otherwise we send an empty list of features
00243                             if (*(t_->current_state()) == 3) {
00244                               t_->updateMovingEdgeSites(sites);
00245                             }
00246 
00247                             sites->header = image_header_;
00248                             moving_edge_sites_publisher.publish(sites);
00249                         }
00250 
00251                         // Publish KLT points.
00252                         if (klt_points_publisher.getNumSubscribers      () > 0)
00253                         {
00254                             visp_tracker::KltPointsPtr klt (new visp_tracker::KltPoints);
00255                             // Test if we are in the state tracking::TrackModel. In that case the pose is good;
00256                             // we can send the klt points. Otherwise we send an empty list of features
00257                             if (*(t_->current_state()) == 3) {
00258                               t_->updateKltPoints(klt);
00259                             }
00260                             klt->header = image_header_;
00261                             klt_points_publisher.publish(klt);
00262                         }
00263 
00264                         if (code_message_publisher.getNumSubscribers() > 0)
00265                         {
00266                           std_msgs::String message;
00267                           if (*(t_->current_state()) == 3) { // Tracking successful
00268 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00269                             message.data = detector->get_message();
00270 #else
00271                             message.data = detector->getMessage( cmd_.get_code_message_index() );
00272 #endif
00273                           }
00274                           else {
00275                             message.data = std::string();
00276                           }
00277                           code_message_publisher.publish(message);
00278                           ROS_INFO_STREAM("Code with message \"" <<  message.data << "\" under tracking");
00279                         }
00280 
00281                         ros::spinOnce();
00282                         rate.sleep();
00283                         if (cmd_.show_fps())
00284                           std::cout << "Tracking done in " << vpTime::measureTimeMs() - t << " ms" << std::endl;
00285                 }
00286                 t_->process_event(tracking::finished());
00287         }
00288 }


visp_auto_tracker
Author(s): Filip Novotny
autogenerated on Thu Jul 4 2019 19:31:08