Go to the documentation of this file.00001 #ifndef HRL_TABLE_DETECTION_TABLETOP_DETECTOR_H
00002 #define HRL_TABLE_DETECTION_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 <boost/thread/mutex.hpp>
00012 #include <cv_bridge/cv_bridge.h>
00013 #include <opencv/cv.h>
00014 #include <opencv2/imgproc/imgproc.hpp>
00015 #include <opencv2/highgui/highgui.hpp>
00016 #include <image_transport/image_transport.h>
00017 #include <sensor_msgs/image_encodings.h>
00018 #include <geometry_msgs/PoseArray.h>
00019 #include <hrl_table_detect/DetectTableStart.h>
00020 #include <hrl_table_detect/DetectTableStop.h>
00021 #include <hrl_table_detect/DetectTableInst.h>
00022 #include <LinearMath/btMatrix3x3.h>
00023 #include <LinearMath/btQuaternion.h>
00024 #include <hrl_cvblobslib/Blob.h>
00025 #include <hrl_cvblobslib/BlobResult.h>
00026
00027 using namespace sensor_msgs;
00028 using namespace std;
00029 namespace enc = sensor_msgs::image_encodings;
00030
00031
00032 namespace hrl_table_detect {
00033 class TabletopDetector {
00034 public:
00035 TabletopDetector();
00036
00037 double minx, maxx, miny, maxy, minz, maxz, resolution, imgx, imgy;
00038 double inlier_magnitude;
00039 int32_t num_edge_dilate, num_closes;
00040 double degree_bins, hough_thresh;
00041 double theta_gran, rho_gran;
00042 double xgran, ygran;
00043
00044 ros::ServiceServer table_detect_start;
00045 ros::ServiceServer table_detect_stop;
00046 ros::ServiceServer table_detect_inst;
00047 ros::Subscriber pc_sub;
00048 ros::NodeHandle nh;
00049 ros::NodeHandle nh_priv;
00050 ros::Publisher pc_pub;
00051 ros::Publisher pose_arr_pub;
00052 cv::Mat height_img_sum, height_img_count, height_img_max;
00053 bool grasp_points_found;
00054 geometry_msgs::PoseArray grasp_points;
00055 image_transport::ImageTransport img_trans;
00056 image_transport::Publisher height_pub;
00057 tf::TransformListener tf_listener;
00058 boost::mutex pc_lock;
00059 void onInit();
00060 void pcCallback(PointCloud2::ConstPtr pc);
00061 bool startCallback(hrl_table_detect::DetectTableStart::Request& req,
00062 hrl_table_detect::DetectTableStart::Response& resp);
00063 bool stopCallback(hrl_table_detect::DetectTableStop::Request& req,
00064 hrl_table_detect::DetectTableStop::Response& resp);
00065 bool instCallback(hrl_table_detect::DetectTableInst::Request& req,
00066 hrl_table_detect::DetectTableInst::Response& resp);
00067 };
00068 };
00069 #endif //HRL_OBJECT_FETCHING_TABLETOP_DETECTOR_H