00001 #ifndef HECTOR_BARREL_DETECTION_NODE_H 00002 #define HECTOR_BARREL_DETECTION_NODE_H 00003 00004 #include <ros/ros.h> 00005 #include <sensor_msgs/PointCloud2.h> 00006 #include <sensor_msgs/CameraInfo.h> 00007 #include <sensor_msgs/Image.h> 00008 #include <image_transport/camera_subscriber.h> 00009 #include <tf/transform_listener.h> 00010 #include <tf_conversions/tf_eigen.h> 00011 #include <hector_barrel_detection/hector_barrel_detection.h> 00012 #include <pcl_conversions/pcl_conversions.h> 00013 #include <pcl/point_types.h> 00014 #include <pcl/PCLPointCloud2.h> 00015 #include <pcl/conversions.h> 00016 #include <pcl_ros/transforms.h> 00017 00018 #include <pcl/io/pcd_io.h> 00019 #include <pcl/filters/passthrough.h> 00020 #include <pcl/filters/voxel_grid.h> 00021 #include <pcl/filters/statistical_outlier_removal.h> 00022 #include <pcl/ModelCoefficients.h> 00023 #include <pcl/features/normal_3d.h> 00024 #include <pcl/sample_consensus/method_types.h> 00025 #include <pcl/sample_consensus/model_types.h> 00026 #include <pcl/segmentation/sac_segmentation.h> 00027 #include <pcl/filters/extract_indices.h> 00028 #include <tf/transform_datatypes.h> 00029 #include <geometry_msgs/PoseStamped.h> 00030 #include <visualization_msgs/MarkerArray.h> 00031 #include <opencv/highgui.h> 00032 #include<opencv/cv.h> 00033 #include <cv_bridge/cv_bridge.h> 00034 #include <sensor_msgs/image_encodings.h> 00035 #include <hector_worldmodel_msgs/ImagePercept.h> 00036 #include <hector_worldmodel_msgs/PosePercept.h> 00037 #include <hector_worldmodel_msgs/GetObjectModel.h> 00038 #include <image_transport/image_transport.h> 00039 #include <message_filters/subscriber.h> 00040 #include <hector_nav_msgs/GetDistanceToObstacle.h> 00041 00042 #include <message_filters/sync_policies/approximate_time.h> 00043 #include <image_geometry/pinhole_camera_model.h> 00044 00045 namespace barrel_detection{ 00046 00047 class BarrelDetection { 00048 00049 public: 00050 BarrelDetection(); 00051 virtual ~BarrelDetection(); 00052 protected: 00053 void PclCallback(const sensor_msgs::PointCloud2::ConstPtr& pc_msg); 00054 void imageCallback(const sensor_msgs::ImageConstPtr& img, const sensor_msgs::CameraInfoConstPtr& info); 00055 void findCylinder(const sensor_msgs::PointCloud2::ConstPtr& pc_msg, float xKey, float yKey); 00056 00057 00058 private: 00059 typedef boost::shared_ptr<image_geometry::PinholeCameraModel> CameraModelPtr; 00060 00061 ros::Subscriber pcl_sub; 00062 image_transport::CameraSubscriber image_sub; 00063 ros::Publisher cloud_filtered_publisher_; 00064 ros::Publisher pose_publisher_; 00065 ros::Publisher barrel_marker_publisher_; 00066 ros::Publisher imagePercept_pub_; 00067 ros::Publisher posePercept_pub_; 00068 ros::ServiceClient worldmodel_srv_client_; 00069 tf::TransformListener listener_; 00070 Eigen::Affine3d to_map_; 00071 pcl::PassThrough<pcl::PointXYZ> pass_; 00072 00073 ros::Publisher debug_imagePoint_pub_; 00074 ros::Publisher pcl_debug_pub_; 00075 00076 sensor_msgs::PointCloud2::ConstPtr current_pc_msg_; 00077 00078 int r_min; 00079 int r_max; 00080 int g_min; 00081 int g_max; 00082 int b_min; 00083 int b_max; 00084 00085 }; 00086 } 00087 00088 namespace head_tracking_mode{ 00089 const unsigned char NONE = 0; 00090 const unsigned char LEFT_HAND_TRACKING = 1; 00091 const unsigned char RIGHT_HAND_TRACKING = 2; 00092 } 00093 #endif // HECTOR_BARREL_DETECTION_NODE_H