00001 #ifndef __VISP_AUTO_TRACKER_NODE_H__ 00002 #define __VISP_AUTO_TRACKER_NODE_H__ 00003 #include "ros/ros.h" 00004 00005 #include "visp/vpConfig.h" 00006 #include "libauto_tracker/tracking.h" 00007 #include <string> 00008 00009 #include "message_filters/subscriber.h" 00010 #include "message_filters/time_synchronizer.h" 00011 #include "sensor_msgs/CameraInfo.h" 00012 #include "sensor_msgs/Image.h" 00013 #include "geometry_msgs/PoseStamped.h" 00014 #include "geometry_msgs/PoseWithCovarianceStamped.h" 00015 #include "std_msgs/Header.h" 00016 #include <sstream> 00017 00018 namespace visp_auto_tracker{ 00019 class Node{ 00020 private: 00021 boost::mutex lock_; 00022 ros::NodeHandle n_; 00023 unsigned long queue_size_; 00024 std::string tracker_config_path_; 00025 std::string model_description_; 00026 std::string model_path_; 00027 std::string model_name_; 00028 std::string code_message_; 00029 bool debug_display_; 00030 00031 vpImage<vpRGBa> I_; // Image used for debug display 00032 std_msgs::Header image_header_; 00033 bool got_image_; 00034 vpCameraParameters cam_; 00035 00036 tracking::Tracker* t_; 00037 CmdLine cmd_; 00038 00039 void waitForImage(); 00040 00041 void frameCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& cam_info); 00042 00043 public: 00044 Node(); 00045 void spin(); 00046 }; 00047 }; 00048 #endif