00001 #ifndef HRL_OBJECT_FETCHING_TABLETOP_DETECTOR_H 00002 #define HRL_OBJECT_FETCHING_TABLETOP_DETECTOR_H 00003 #include <numeric> 00004 #include <ros/ros.h> 00005 #include "sensor_msgs/PointCloud2.h" 00006 #include <pcl_ros/point_cloud.h> 00007 #include <tf/transform_listener.h> 00008 #include "pcl_ros/transforms.h" 00009 #include "pcl/point_types.h" 00010 #include <boost/make_shared.hpp> 00011 #include <cv_bridge/cv_bridge.h> 00012 #include <opencv/cv.h> 00013 #include <opencv2/imgproc/imgproc.hpp> 00014 #include <opencv2/highgui/highgui.hpp> 00015 #include <image_transport/image_transport.h> 00016 #include <sensor_msgs/image_encodings.h> 00017 #include <geometry_msgs/PoseArray.h> 00018 #include <hrl_object_fetching/DetectTable.h> 00019 #include <LinearMath/btMatrix3x3.h> 00020 #include <LinearMath/btQuaternion.h> 00021 00022 using namespace sensor_msgs; 00023 using namespace std; 00024 namespace enc = sensor_msgs::image_encodings; 00025 00026 00027 namespace hrl_object_fetching { 00028 class TabletopDetector { 00029 public: 00030 TabletopDetector(); 00031 ros::ServiceServer table_detect_service; 00032 ros::Subscriber pc_sub; 00033 ros::NodeHandle nh; 00034 ros::NodeHandle nh_priv; 00035 ros::Publisher pc_pub; 00036 ros::Publisher pose_arr_pub; 00037 bool grasp_points_found; 00038 geometry_msgs::PoseArray grasp_points; 00039 image_transport::ImageTransport img_trans; 00040 image_transport::Publisher height_pub; 00041 tf::TransformListener tf_listener; 00042 void onInit(); 00043 void pcCallback(PointCloud2::ConstPtr pc); 00044 bool srvCallback(hrl_object_fetching::DetectTable::Request& req, 00045 hrl_object_fetching::DetectTable::Response& resp); 00046 }; 00047 }; 00048 #endif //HRL_OBJECT_FETCHING_TABLETOP_DETECTOR_H