00001 00013 #ifndef RAIL_PICK_AND_PLACE_OBJECT_RECOGNITION_LISTENER_H_ 00014 #define RAIL_PICK_AND_PLACE_OBJECT_RECOGNITION_LISTENER_H_ 00015 00016 // ROS 00017 #include <graspdb/graspdb.h> 00018 #include <rail_manipulation_msgs/SegmentedObjectList.h> 00019 #include <rail_pick_and_place_msgs/RemoveObject.h> 00020 #include <rail_recognition/ImageRecognizer.h> 00021 #include <ros/ros.h> 00022 #include <tf/transform_datatypes.h> 00023 00024 // PCL 00025 #include <pcl/filters/project_inliers.h> 00026 #include <pcl/filters/voxel_grid.h> 00027 00028 // Boost 00029 #include <boost/thread/mutex.hpp> 00030 00031 namespace rail 00032 { 00033 namespace pick_and_place 00034 { 00035 00043 class ObjectRecognitionListener 00044 { 00045 public: 00047 static const bool DEFAULT_DEBUG = false; 00049 static const float DOWNSAMPLE_LEAF_SIZE = 0.01; 00051 static const double SAME_OBJECT_DIST_THRESHOLD = 0.2; 00053 static const double RECOGNITION_THRESHOLD = 0.5; 00054 00060 ObjectRecognitionListener(); 00061 00067 virtual ~ObjectRecognitionListener(); 00068 00076 bool okay() const; 00077 00078 private: 00087 void segmentedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectList::ConstPtr &objects); 00088 00098 bool comparePointClouds(const sensor_msgs::PointCloud2 &pc1, const sensor_msgs::PointCloud2 &pc2) const; 00099 00110 void combineModels(const rail_manipulation_msgs::SegmentedObject &model1, 00111 const rail_manipulation_msgs::SegmentedObject &model2, rail_manipulation_msgs::SegmentedObject &combined) const; 00112 00123 bool removeObjectCallback(rail_pick_and_place_msgs::RemoveObject::Request &req, 00124 rail_pick_and_place_msgs::RemoveObject::Response &res); 00125 00127 bool debug_, okay_; 00129 graspdb::Client *graspdb_; 00130 00132 boost::mutex mutex_; 00133 00135 ros::NodeHandle node_, private_node_; 00137 ros::Subscriber segmented_objects_sub_; 00139 ros::Publisher recognized_objects_pub_, debug_pub_; 00141 ros::ServiceServer remove_object_srv_; 00143 rail_manipulation_msgs::SegmentedObjectList object_list_; 00144 00145 ImageRecognizer image_recognizer_; 00146 00147 bool use_image_recognition_; 00148 }; 00149 00150 } 00151 } 00152 00153 #endif