callbacks.cpp
Go to the documentation of this file.
00001 #include <stdexcept>
00002 #include <boost/bind.hpp>
00003 #include <image_transport/image_transport.h>
00004 #include <sensor_msgs/Image.h>
00005 #include <visp/vpImage.h>
00006 
00007 #include <visp_tracker/Init.h>
00008 
00009 #include "names.hh"
00010 #include "conversion.hh"
00011 #include "callbacks.hh"
00012 
00013 # include <visp/vpMbEdgeTracker.h>
00014 # include <visp/vpMbKltTracker.h>
00015 
00016 void imageCallback(vpImage<unsigned char>& image,
00017                    const sensor_msgs::Image::ConstPtr& msg,
00018                    const sensor_msgs::CameraInfoConstPtr& info)
00019 {
00020   try
00021     {
00022       rosImageToVisp(image, msg);
00023     }
00024   catch(std::exception& e)
00025     {
00026       ROS_ERROR_STREAM("dropping frame: " << e.what());
00027     }
00028 }
00029 
00030 void imageCallback(vpImage<unsigned char>& image,
00031                    std_msgs::Header& header,
00032                    sensor_msgs::CameraInfoConstPtr& info,
00033                    const sensor_msgs::Image::ConstPtr& msg,
00034                    const sensor_msgs::CameraInfoConstPtr& infoConst)
00035 {
00036   imageCallback(image, msg, info);
00037   header = msg->header;
00038   info = infoConst;
00039 }
00040 
00041 image_transport::CameraSubscriber::Callback
00042 bindImageCallback(vpImage<unsigned char>& image)
00043 {
00044   return boost::bind(imageCallback, boost::ref(image), _1, _2);
00045 }
00046 
00047 image_transport::CameraSubscriber::Callback
00048 bindImageCallback(vpImage<unsigned char>& image,
00049                   std_msgs::Header& header,
00050                   sensor_msgs::CameraInfoConstPtr& info)
00051 {
00052   return boost::bind
00053     (imageCallback,
00054      boost::ref(image), boost::ref(header), boost::ref(info), _1, _2);
00055 }
00056 
00057 void reconfigureCallback(vpMbTracker* tracker,
00058        vpImage<unsigned char>& I,
00059        vpMe& moving_edge,
00060        vpKltOpencv& kltTracker,
00061        boost::recursive_mutex& mutex,
00062        visp_tracker::ModelBasedSettingsConfig& config,
00063        uint32_t level)
00064 {
00065   mutex.lock ();
00066   try
00067     {
00068       ROS_INFO("Reconfigure Model Based Hybrid Tracker request received.");
00069 
00070       convertModelBasedSettingsConfigToVpMbTracker<visp_tracker::ModelBasedSettingsConfig>(config, tracker);
00071 
00072       convertModelBasedSettingsConfigToVpMe<visp_tracker::ModelBasedSettingsConfig>(config, moving_edge, tracker);
00073 //         moving_edge.print();
00074 
00075       convertModelBasedSettingsConfigToVpKltOpencv<visp_tracker::ModelBasedSettingsConfig>(config, kltTracker, tracker);
00076       
00077       vpHomogeneousMatrix cMo;
00078       tracker->getPose(cMo);
00079 
00080 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00081       // Work arround for ViSP 2.9.0 to allow dynamic reconfigure to work
00082       // when hybrid tracker is in use and moving edges settings are changed
00083       vpMbKltTracker* tracker_klt = dynamic_cast<vpMbKltTracker*>(tracker);
00084       if (tracker_klt != NULL)
00085         tracker_klt->firstTrack = true;
00086 #endif
00087 
00088       // Check if the image is ready to use
00089       if (I.getHeight() != 0 && I.getWidth() != 0) {
00090 #if VISP_VERSION_INT < VP_VERSION_INT(3,0,0)
00091         // Could not use just initFromPose() for hybrid tracker
00092         // init() function from edge tracker has to be fixed in the trunk first
00093         // It might have to reset the meLines
00094         tracker->setPose(I, cMo);
00095 #endif
00096         tracker->initFromPose(I, cMo);
00097       }
00098     }
00099   catch (...)
00100     {
00101       mutex.unlock ();
00102       throw;
00103     }
00104   mutex.unlock ();
00105 }
00106 
00107 void reconfigureEdgeCallback(vpMbTracker* tracker,
00108        vpImage<unsigned char>& I,
00109        vpMe& moving_edge,
00110        boost::recursive_mutex& mutex,
00111        visp_tracker::ModelBasedSettingsEdgeConfig& config,
00112        uint32_t level)
00113 {
00114 
00115   mutex.lock ();
00116   try
00117     {
00118       ROS_INFO("Reconfigure Model Based Edge Tracker request received.");
00119 
00120       convertModelBasedSettingsConfigToVpMbTracker<visp_tracker::ModelBasedSettingsEdgeConfig>(config, tracker);
00121       convertModelBasedSettingsConfigToVpMe<visp_tracker::ModelBasedSettingsEdgeConfig>(config, moving_edge, tracker);
00122       // moving_edge.print();
00123 
00124       // Check if the image is ready to use
00125       if (I.getHeight() != 0 && I.getWidth() != 0) {
00126         vpHomogeneousMatrix cMo;
00127         tracker->getPose(cMo);
00128         // Could not use initFromPose for edge tracker
00129         // init() function has to be fixed in the trunk first
00130         // It might have to reset the meLines
00131         tracker->setPose(I, cMo);
00132       }
00133     }
00134   catch (...)
00135     {
00136       mutex.unlock ();
00137       throw;
00138     }
00139   mutex.unlock ();
00140 }
00141 
00142 void reconfigureKltCallback(vpMbTracker* tracker,
00143        vpImage<unsigned char>& I,
00144        vpKltOpencv& kltTracker,
00145        boost::recursive_mutex& mutex,
00146        visp_tracker::ModelBasedSettingsKltConfig& config,
00147        uint32_t level)
00148 {
00149   mutex.lock ();
00150   try
00151     {
00152       ROS_INFO("Reconfigure Model Based KLT Tracker request received.");
00153 
00154       convertModelBasedSettingsConfigToVpMbTracker<visp_tracker::ModelBasedSettingsKltConfig>(config, tracker);
00155       convertModelBasedSettingsConfigToVpKltOpencv<visp_tracker::ModelBasedSettingsKltConfig>(config, kltTracker, tracker);
00156 
00157       // Check if the image is ready to use
00158       if (I.getHeight() != 0 && I.getWidth() != 0) {
00159         vpHomogeneousMatrix cMo;
00160         tracker->getPose(cMo);
00161         tracker->initFromPose(I, cMo);
00162       }
00163     }
00164   catch (...)
00165     {
00166       mutex.unlock ();
00167       throw;
00168     }
00169   mutex.unlock ();
00170 }
00171 
00172 void reInitViewerCommonParameters(ros::NodeHandle& nh,
00173                                   vpMbTracker* tracker)
00174 {
00175   ros::ServiceClient clientViewer =
00176       nh.serviceClient<visp_tracker::Init>(visp_tracker::reconfigure_service_viewer);
00177   visp_tracker::Init srv;
00178   convertVpMbTrackerToInitRequest(tracker, srv);
00179   if (clientViewer.call(srv))
00180   {
00181     if (srv.response.initialization_succeed)
00182       ROS_INFO("Tracker Viewer initialized with success.");
00183     else
00184       throw std::runtime_error("failed to initialize tracker viewer.");
00185   }
00186 }
00187 
00188 void reconfigureCallbackAndInitViewer(ros::NodeHandle& nh,
00189        vpMbTracker* tracker,
00190        vpImage<unsigned char>& I,
00191        vpMe& moving_edge,
00192        vpKltOpencv& kltTracker,
00193        boost::recursive_mutex& mutex,
00194        visp_tracker::ModelBasedSettingsConfig& config,
00195        uint32_t level)
00196 {
00197   reconfigureCallback(tracker,I,moving_edge,kltTracker,mutex,config,level);
00198   reInitViewerCommonParameters(nh,tracker);
00199 }
00200 
00201 void reconfigureEdgeCallbackAndInitViewer(ros::NodeHandle& nh,
00202        vpMbTracker* tracker,
00203        vpImage<unsigned char>& I,
00204        vpMe& moving_edge,
00205        boost::recursive_mutex& mutex,
00206        visp_tracker::ModelBasedSettingsEdgeConfig& config,
00207        uint32_t level)
00208 {
00209   reconfigureEdgeCallback(tracker,I,moving_edge,mutex,config,level);
00210   reInitViewerCommonParameters(nh,tracker);
00211 }
00212 
00213 void reconfigureKltCallbackAndInitViewer(ros::NodeHandle& nh,
00214        vpMbTracker* tracker,
00215        vpImage<unsigned char>& I,
00216        vpKltOpencv& kltTracker,
00217        boost::recursive_mutex& mutex,
00218        visp_tracker::ModelBasedSettingsKltConfig& config,
00219        uint32_t level)
00220 {
00221   reconfigureKltCallback(tracker,I,kltTracker,mutex,config,level);
00222   reInitViewerCommonParameters(nh,tracker);
00223 }


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