tabletop_segmentor.h
Go to the documentation of this file.
00001 #ifndef HRL_TABLE_DETECTION_TABLETOP_SEGMENTOR_H
00002 #define HRL_TABLE_DETECTION_TABLETOP_SEGMENTOR_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 namespace hrl_table_detect {
00032     class TabletopSegmentor {
00033         public:
00034             TabletopSegmentor();
00035             // Parameters
00036             double minx, maxx, miny, maxy, minz, maxz, resolution, imgx, imgy;
00037             double inlier_magnitude; 
00038             int32_t num_edge_dilate, num_closes;
00039             double degree_bins, hough_thresh;
00040             double theta_gran, rho_gran;
00041             double xgran, ygran;
00042             double obj_min_area;
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 obj_arr_pub;
00052             cv::Mat height_img_sum, height_img_count, height_img_max;
00053             bool obj_poses_found;
00054             geometry_msgs::PoseArray obj_poses;
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


hrl_table_detect
Author(s): kelsey
autogenerated on Wed Nov 27 2013 12:15:26