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 //detectors
00008 #include "detectors/datamatrix/detector.h"
00009 #include "detectors/qrcode/detector.h"
00010 
00011 //tracking
00012 #include "libauto_tracker/tracking.h"
00013 #include "libauto_tracker/threading.h"
00014 #include "libauto_tracker/events.h"
00015 
00016 #include "visp_tracker/MovingEdgeSites.h"
00017 #include "visp_tracker/KltPoints.h"
00018 
00019 //visp includes
00020 #include <visp/vpDisplayX.h>
00021 #include <visp/vpMbEdgeKltTracker.h>
00022 #include <visp/vpMbKltTracker.h>
00023 #include <visp/vpMbEdgeTracker.h>
00024 #include <visp/vpTime.h>
00025 
00026 #include "conversions/camera.h"
00027 #include "conversions/image.h"
00028 #include "conversions/3dpose.h"
00029 
00030 #include "libauto_tracker/tracking.h"
00031 
00032 #include "resource_retriever/retriever.h"
00033 
00034 #include "std_msgs/Int8.h"
00035 
00036 
00037 namespace visp_auto_tracker{
00038         Node::Node() :
00039                         n_("~"),
00040                         queue_size_(1),
00041                         got_image_(false){
00042                 //get the tracker configuration file
00043                 //this file contains all of the tracker's parameters, they are not passed to ros directly.
00044                 n_.param<std::string>("tracker_config_path", tracker_config_path_, "");
00045                 n_.param<bool>("debug_display", debug_display_, false);
00046                 std::string model_name;
00047                 std::string model_full_path;
00048                 n_.param<std::string>("model_path", model_path_, "");
00049                 n_.param<std::string>("model_name", model_name, "");
00050                 model_path_= model_path_[model_path_.length()-1]=='/'?model_path_:model_path_+std::string("/");
00051                 model_full_path = model_path_+model_name;
00052                 tracker_config_path_ = model_full_path+".cfg";
00053                 ROS_INFO("model full path=%s",model_full_path.c_str());
00054                 resource_retriever::Retriever r;
00055                 resource_retriever::MemoryResource res = r.get(std::string("file://")+std::string(model_full_path+".wrl"));
00056 
00057                 model_description_.resize(res.size);
00058                 unsigned i = 0;
00059                 for (; i < res.size; ++i)
00060                         model_description_[i] = res.data.get()[i];
00061 
00062                 ROS_INFO("model content=%s",model_description_.c_str());
00063 
00064                 n_.setParam ("/model_description", model_description_);
00065 
00066         }
00067 
00068         void Node::waitForImage(){
00069             while ( ros::ok ()){
00070                 if(got_image_) return;
00071                 ros::spinOnce();
00072               }
00073         }
00074 
00075         //records last recieved image
00076         void Node::frameCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& cam_info){
00077                 boost::mutex::scoped_lock(lock_);
00078                 image_header_ = image->header;
00079                 I_ = visp_bridge::toVispImageRGBa(*image); //make sure the image isn't worked on by locking a mutex
00080                 cam_ = visp_bridge::toVispCameraParameters(*cam_info);
00081 
00082                 got_image_ = true;
00083         }
00084 
00085         void Node::spin(){
00086                 //Parse command line arguments from config file (as ros param)
00087                 CmdLine cmd(tracker_config_path_);
00088                 cmd.set_data_directory(model_path_); //force data path
00089 
00090                 if(cmd.should_exit()) return; //exit if needed
00091 
00092                 vpMbTracker* tracker; //mb-tracker will be chosen according to config
00093 
00094                 //create display
00095                 vpDisplayX* d = NULL;
00096                 if(debug_display_)
00097                   d = new vpDisplayX();
00098 
00099                 //init detector based on user preference
00100                 detectors::DetectorBase* detector = NULL;
00101                 if (cmd.get_detector_type() == CmdLine::ZBAR)
00102                         detector = new detectors::qrcode::Detector;
00103                 else if(cmd.get_detector_type() == CmdLine::DMTX)
00104                         detector = new detectors::datamatrix::Detector;
00105 
00106 #if 0
00107                 //init tracker based on user preference
00108                 if(cmd.get_tracker_type() == CmdLine::KLT)
00109                         tracker = new vpMbKltTracker();
00110                 else if(cmd.get_tracker_type() == CmdLine::KLT_MBT)
00111                         tracker = new vpMbEdgeKltTracker();
00112                 else if(cmd.get_tracker_type() == CmdLine::MBT)
00113                         tracker = new vpMbEdgeTracker();
00114 #else
00115                 // Use the best tracker
00116                 tracker = new vpMbEdgeKltTracker();
00117 #endif
00118                 tracker->setCameraParameters(cam_);
00119                 tracker->setDisplayFeatures(true);
00120 
00121                 //compile detectors and paramters into the automatic tracker.
00122                 t_ = new tracking::Tracker(cmd, detector, tracker, debug_display_);
00123                 t_->start(); //start the state machine
00124 
00125                 //subscribe to ros topics and prepare a publisher that will publish the pose
00126                 message_filters::Subscriber<sensor_msgs::Image> raw_image_subscriber(n_, image_topic, queue_size_);
00127                 message_filters::Subscriber<sensor_msgs::CameraInfo> camera_info_subscriber(n_, camera_info_topic, queue_size_);
00128                 message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> image_info_sync(raw_image_subscriber, camera_info_subscriber, queue_size_);
00129                 image_info_sync.registerCallback(boost::bind(&Node::frameCallback,this, _1, _2));
00130                 ros::Publisher object_pose_publisher = n_.advertise<geometry_msgs::PoseStamped>(object_position_topic, queue_size_);
00131                 ros::Publisher object_pose_covariance_publisher = n_.advertise<geometry_msgs::PoseWithCovarianceStamped>(object_position_covariance_topic, queue_size_);
00132                 ros::Publisher moving_edge_sites_publisher = n_.advertise<visp_tracker::MovingEdgeSites>(moving_edge_sites_topic, queue_size_);
00133                 ros::Publisher klt_points_publisher = n_.advertise<visp_tracker::KltPoints>(klt_points_topic, queue_size_);
00134                 ros::Publisher status_publisher = n_.advertise<std_msgs::Int8>(status_topic, queue_size_);
00135 
00136                 //wait for an image to be ready
00137                 waitForImage();
00138                 {
00139                         //when an image is ready tell the tracker to start searching for patterns
00140                         boost::mutex::scoped_lock(lock_);
00141                         if(debug_display_) {
00142                           d->init(I_); //also init display
00143                           vpDisplay::setTitle(I_, "visp_auto_tracker debug display");
00144                         }
00145 
00146                         t_->process_event(tracking::select_input(I_));
00147                 }
00148 
00149                 unsigned int iter=0;
00150                 geometry_msgs::PoseStamped ps;
00151                 geometry_msgs::PoseWithCovarianceStamped ps_cov;
00152 
00153                 ros::Rate rate(30); //init 25fps publishing frequency
00154                 while(ros::ok()){
00155                   double t = vpTime::measureTimeMs();
00156                         boost::mutex::scoped_lock(lock_);
00157                         //process the new frame with the tracker
00158                         t_->process_event(tracking::input_ready(I_,cam_,iter));
00159                         //When the tracker is tracking, it's in the tracking::TrackModel state
00160                         //Access this state and broadcast the pose
00161                         tracking::TrackModel& track_model = t_->get_state<tracking::TrackModel&>();
00162 
00163                         ps.pose = visp_bridge::toGeometryMsgsPose(track_model.cMo); //convert
00164 
00165                         // Publish resulting pose.
00166                         if (object_pose_publisher.getNumSubscribers     () > 0)
00167                         {
00168                             ps.header = image_header_;
00169                             ps.header.frame_id = tracker_ref_frame;
00170                             object_pose_publisher.publish(ps);
00171                         }
00172 
00173                         // Publish resulting pose covariance matrix.
00174                         if (object_pose_covariance_publisher.getNumSubscribers  () > 0)
00175                         {
00176                             ps_cov.pose.pose = ps.pose;
00177                             ps_cov.header = image_header_;
00178                             ps_cov.header.frame_id = tracker_ref_frame;
00179                             object_pose_covariance_publisher.publish(ps_cov);
00180                         }
00181 
00182                         // Publish state machine status.
00183                         if (status_publisher.getNumSubscribers  () > 0)
00184                         {
00185                             std_msgs::Int8 status;
00186                             status.data = (unsigned char)(*(t_->current_state()));
00187                             status_publisher.publish(status);
00188                         }
00189 
00190                         // Publish moving edge sites.
00191                         if (moving_edge_sites_publisher.getNumSubscribers       () > 0)
00192                         {
00193                             visp_tracker::MovingEdgeSitesPtr sites
00194                                     (new visp_tracker::MovingEdgeSites);
00195                             t_->updateMovingEdgeSites(sites);
00196                             sites->header = image_header_;
00197                             moving_edge_sites_publisher.publish(sites);
00198                         }
00199 
00200                         // Publish KLT points.
00201                         if (klt_points_publisher.getNumSubscribers      () > 0)
00202                         {
00203                             visp_tracker::KltPointsPtr klt
00204                                     (new visp_tracker::KltPoints);
00205                             t_->updateKltPoints(klt);
00206                             klt->header = image_header_;
00207                             klt_points_publisher.publish(klt);
00208                         }
00209 
00210                         ros::spinOnce();
00211                         rate.sleep();
00212                         if (cmd.show_fps())
00213                           std::cout << "Tracking done in " << vpTime::measureTimeMs() - t << " ms" << std::endl;
00214                 }
00215                 t_->process_event(tracking::finished());
00216         }
00217 }


visp_auto_tracker
Author(s): Filip Novotny, Fabien Spindler/Fabien.Spindler@inria.fr
autogenerated on Sat Dec 28 2013 17:46:16