00001 #include <ros/ros.h> 00002 #include <ros/callback_queue.h> 00003 #include <iostream> 00004 #include <vector> 00005 #include <string> 00006 #include <exception> 00007 #include <cstdlib> 00008 00009 #include <boost/thread/thread.hpp> 00010 #include <boost/thread/mutex.hpp> 00011 #include <boost/thread/recursive_mutex.hpp> 00012 #include <Eigen/Geometry> 00013 00014 #include <pcl/visualization/pcl_visualizer.h> 00015 #include <tidyup_msgs/DetectGraspableObjects.h> 00016 #include <narf_recognition/object_recognition_helper.h> 00017 00018 class ObjectRecognitionServer { 00019 public: 00020 00021 ObjectRecognitionServer(ros::NodeHandle& n); 00022 ~ObjectRecognitionServer(); 00023 00024 ros::NodeHandle nh_; 00025 00026 float noise_level_factor_; // Factor regarding average model radius 00027 float support_size_factor_; // Factor regarding average model radius 00028 float minimum_plane_size_; 00029 float initial_max_plane_error_; 00030 bool show_found_objects_; 00031 bool cloud_received_; 00032 bool cloud_thread_shutdown_; 00033 bool visualize_result_; 00034 00035 ros::NodeHandle cloud_nh_; 00036 boost::thread * cloud_thread_; 00037 ros::CallbackQueue cloud_callback_queue_; 00038 boost::recursive_mutex mutex_; 00039 00040 ros::Subscriber sub_; 00041 ros::ServiceServer serv_; 00042 ros::Publisher marker_pub_; 00043 00044 pcl::ObjectRecognitionHelper object_recognition_helper_; 00045 pcl::ObjectModelList object_models_; 00046 pcl::PointCloud<pcl::PointXYZ> point_cloud_; 00047 pcl::visualization::PCLVisualizer* viewer_; 00048 00049 std::vector<std::string> modelnames_; 00050 00051 void runThreadCallbackLoop(); 00052 void getModels(std::vector<std::string> model_filenames); 00053 void cloud_cb_(const sensor_msgs::PointCloud2 msg); 00054 void visualize(pcl::visualization::PCLVisualizer& viewer, 00055 std::vector<pcl::PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model, 00056 std::vector<std::string>& point_clouds_in_viewer); 00057 bool locate(tidyup_msgs::DetectGraspableObjects::Request &req, 00058 tidyup_msgs::DetectGraspableObjects::Response &res); 00059 };
