00001 #ifndef HECTOR_BARREL_DETECTION_NODE_H 00002 #define HECTOR_BARREL_DETECTION_NODE_H 00003 00004 #include <ros/ros.h> 00005 #include <nodelet/nodelet.h> 00006 #include <sensor_msgs/PointCloud2.h> 00007 #include <sensor_msgs/CameraInfo.h> 00008 #include <sensor_msgs/Image.h> 00009 #include <sensor_msgs/image_encodings.h> 00010 #include <image_transport/camera_subscriber.h> 00011 #include <tf/transform_listener.h> 00012 #include <tf_conversions/tf_eigen.h> 00013 #include <pcl_conversions/pcl_conversions.h> 00014 #include <pcl/point_types.h> 00015 #include <pcl/PCLPointCloud2.h> 00016 #include <pcl/conversions.h> 00017 #include <pcl_ros/transforms.h> 00018 00019 #include <pcl/io/pcd_io.h> 00020 #include <pcl/filters/passthrough.h> 00021 #include <pcl/filters/voxel_grid.h> 00022 #include <pcl/filters/statistical_outlier_removal.h> 00023 #include <pcl/ModelCoefficients.h> 00024 #include <pcl/features/normal_3d.h> 00025 #include <pcl/sample_consensus/method_types.h> 00026 #include <pcl/sample_consensus/model_types.h> 00027 #include <pcl/segmentation/sac_segmentation.h> 00028 #include <pcl/filters/extract_indices.h> 00029 #include <tf/transform_datatypes.h> 00030 #include <geometry_msgs/PoseStamped.h> 00031 #include <visualization_msgs/MarkerArray.h> 00032 #include <opencv/highgui.h> 00033 #include <opencv/cv.h> 00034 #include <cv_bridge/cv_bridge.h> 00035 #include <sensor_msgs/image_encodings.h> 00036 #include <hector_worldmodel_msgs/ImagePercept.h> 00037 #include <hector_worldmodel_msgs/PosePercept.h> 00038 #include <hector_worldmodel_msgs/GetObjectModel.h> 00039 #include <image_transport/image_transport.h> 00040 #include <message_filters/subscriber.h> 00041 #include <hector_nav_msgs/GetDistanceToObstacle.h> 00042 00043 #include <message_filters/sync_policies/approximate_time.h> 00044 #include <image_geometry/pinhole_camera_model.h> 00045 #include <dynamic_reconfigure/server.h> 00046 #include <hector_barrel_detection_nodelet/BarrelDetectionConfig.h> 00047 00048 namespace hector_barrel_detection_nodelet{ 00049 00050 class BarrelDetection : public nodelet::Nodelet { 00051 00052 public: 00053 virtual void onInit(); 00054 BarrelDetection(); 00055 virtual ~BarrelDetection(); 00056 protected: 00057 void PclCallback(const sensor_msgs::PointCloud2::ConstPtr& pc_msg); 00058 void imageCallback(const sensor_msgs::ImageConstPtr& img, const sensor_msgs::CameraInfoConstPtr& info); 00059 void findCylinder(const sensor_msgs::PointCloud2::ConstPtr& pc_msg, float xKey, float yKey, const geometry_msgs::PointStamped cut_around_keypoint); 00060 00061 00062 private: 00063 typedef boost::shared_ptr<image_geometry::PinholeCameraModel> CameraModelPtr; 00064 00065 ros::Subscriber pcl_sub; 00066 image_transport::CameraSubscriber image_sub; 00067 ros::Publisher cloud_filtered_publisher_; 00068 ros::Publisher pose_publisher_; 00069 ros::Publisher barrel_marker_publisher_; 00070 ros::Publisher imagePercept_pub_; 00071 ros::Publisher posePercept_pub_; 00072 ros::ServiceClient worldmodel_srv_client_; 00073 tf::TransformListener listener_; 00074 Eigen::Affine3d to_map_; 00075 pcl::PassThrough<pcl::PointXYZ> pass_; 00076 00077 ros::Publisher debug_imagePoint_pub_; 00078 ros::Publisher pcl_debug_pub_; 00079 image_transport::CameraPublisher black_white_image_pub_; 00080 00081 sensor_msgs::PointCloud2::ConstPtr current_pc_msg_; 00082 00083 //Dynamic reconfigure 00084 dynamic_reconfigure::Server<hector_barrel_detection_nodelet::BarrelDetectionConfig> dynamic_recf_server; 00085 dynamic_reconfigure::Server<hector_barrel_detection_nodelet::BarrelDetectionConfig>::CallbackType dynamic_recf_type; 00086 void dynamic_recf_cb(hector_barrel_detection_nodelet::BarrelDetectionConfig &config, uint32_t level); 00087 image_transport::CameraPublisher pub_imageDetection; 00088 void publish_rectangle_for_recf(std::vector<cv::KeyPoint> keypoints, const sensor_msgs::ImageConstPtr &img, const sensor_msgs::CameraInfoConstPtr &info, cv::Mat &img_filtered); 00089 00090 int v_min; 00091 int v_max; 00092 int s_min; 00093 int s_max; 00094 int h_min; 00095 int h_max; 00096 double bluePart; 00097 double minRadius; 00098 double maxRadius; 00099 00100 }; 00101 } 00102 00103 #endif // HECTOR_BARREL_DETECTION_NODE_H