iri_door_detector: closed_door_detector_alg

Go to the documentation of this file.
00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
00002 // Author 
00003 // All rights reserved.
00004 //
00005 // This file is part of iri-ros-pkg
00006 // iri-ros-pkg is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 // 
00019 // IMPORTANT NOTE: This code has been generated through a script from the 
00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
00021 // of the scripts. ROS topics can be easly add by using those scripts. Please
00022 // refer to the IRI wiki page for more information:
00023 // http://wikiri.upc.es/index.php/Robotics_Lab
00024 
00025 #ifndef _closed_door_detector_alg_node_h_
00026 #define _closed_door_detector_alg_node_h_
00027 
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "closed_door_detector_alg.h"
00030 #include <cv_bridge/cv_bridge.h>
00031 #include <ros/ros.h>
00032 #include <image_transport/image_transport.h>
00033 #include <sensor_msgs/image_encodings.h>
00034 #include <opencv2/imgproc/imgproc.hpp>
00035 #include <opencv2/highgui/highgui.hpp>
00036 #include <tf/transform_broadcaster.h>
00037 #include <tf/transform_datatypes.h>
00038 #include <algorithm>
00039 #include <cvblob.h>
00040 
00041 // PCL 
00042 #include <pcl/ros/conversions.h>
00043 #include <pcl/point_cloud.h>
00044 #include <pcl/point_types.h>
00045 #include <pcl/ModelCoefficients.h>
00046 #include <pcl/sample_consensus/method_types.h>
00047 #include <pcl/sample_consensus/model_types.h>
00048 #include <pcl/segmentation/sac_segmentation.h>
00049 #include <pcl/filters/extract_indices.h>
00050 #include <pcl/filters/passthrough.h>
00051 #include <pcl/filters/voxel_grid.h>
00052 #include <pcl/common/transform.h>
00053 #include <pcl/registration/transformation_estimation_svd.h>
00054 
00055 // [publisher subscriber headers]
00056 #include <std_msgs/Int8.h>
00057 #include <std_msgs/Int32MultiArray.h>
00058 #include <geometry_msgs/PoseStamped.h>
00059 #include <sensor_msgs/Image.h>
00060 #include <sensor_msgs/PointCloud2.h>
00061 
00062 // [service client headers]
00063 
00064 // [action server client headers]
00065 
00070 class ClosedDoorDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<ClosedDoorDetectorAlgorithm>
00071 {
00072   private:
00073     // [publisher attributes]
00074     ros::Publisher door_handle_publisher_;
00075     geometry_msgs::PoseStamped PoseStamped_msg_;
00076     ros::Publisher closed_door_coordinates_publisher_;
00077     std_msgs::Int32MultiArray Int32MultiArray_msg_;
00078 
00079     // [subscriber attributes]
00080     ros::Subscriber door_action_start_subscriber_;
00081     void door_action_start_callback(const std_msgs::Int8::ConstPtr& msg);
00082     CMutex door_action_start_mutex_;
00083     ros::Subscriber image_depth_subscriber_;
00084     void image_depth_callback(const sensor_msgs::Image::ConstPtr& msg);
00085     CMutex image_depth_mutex_;
00086     ros::Subscriber points_subscriber_;
00087     void points_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00088     CMutex points_mutex_;
00089     ros::Subscriber image_color_subscriber_;
00090     void image_color_callback(const sensor_msgs::Image::ConstPtr& msg);
00091     CMutex image_color_mutex_;
00092 
00093     // [service attributes]
00094 
00095     // [client attributes]
00096 
00097     // [action server attributes]
00098 
00099     // [action client attributes]
00100 
00101     // [reconfigurable parameters]
00102     int allowed_blobs;
00103     int segment_fidelity;
00104     int segment_size;
00105     int min_door_width;
00106     int max_door_width;
00107     int min_door_height;
00108     int max_door_height;
00109     int handle_width;
00110     int handle_height;
00111     int handle_location;
00112     int debugging_images;
00113     bool no_simulator;
00114     bool DSC;
00115     bool SHM;
00116     bool SVP;
00117     bool SFT;
00118     bool Range_filter;
00119     bool Persp_filter;
00120     bool Geom_filter;
00121     bool Aspect_filter;
00122     bool Size_filter;
00123 
00124     // [algorithm variables]
00125     int compCount;
00126     int frames_size;
00127     int cx_min;
00128     int cx_max;
00129     int cy_min;
00130     int cy_max;
00131     int roi_x;
00132     int roi_y;
00133     int area;
00134     int point;
00135     int f_c1;
00136     int f_c2;
00137     int f_c3;
00138     int f_c4;
00139     int offset_l; 
00140     int offset_r;
00141     int offset_u;
00142     int offset_d;
00143     int check;
00144     int idx;
00145     int repeated_color;
00146     int flat;
00147     int aspect;
00148     int perspective;
00149     int geometry;
00150     int size;
00151     int i_min;
00152     int i_max;
00153     int k_min;
00154     int k_max;
00155     int sensor_range;
00156     int start;
00157     int lower_offset;
00158     int upper_offset;
00159     int captured_depth; 
00160     int handle_x;
00161     int handle_y;
00162     int handle_minx;
00163     int handle_maxx;
00164     int handle_miny;
00165     int handle_maxy;
00166     int door_x;
00167     int door_y;
00168     int support;
00169     double step_x;
00170     double step_y;
00171     double m1;
00172     double m2;
00173     double m3;
00174     double m4;
00175     double b1;
00176     double b2;
00177     double b3;
00178     double b4;
00179     double dx; 
00180     double dy;
00181     double dst; 
00182     double dst_h;
00183     double dst_v;
00184     double min_dst_h;
00185     double min_dst_v;
00186     double max_dst_v;
00187     double max_dst_h;
00188     double dst_up;
00189     double dst_down;
00190     double dst_left;
00191     double dst_right;
00192     double theta;
00193     double theta1;
00194     double theta2;
00195     double theta3; 
00196     double theta4;
00197     double r;
00198     double v_r;
00199     double h_r;
00200     static const double fx_d = 1.0 / 5.9421434211923247e+02;
00201     static const double fy_d = 1.0 / 5.9104053696870778e+02;
00202     static const double cx_d = 3.3930780975300314e+02;
00203     static const double cy_d = 2.4273913761751615e+02;
00204     static const double min_range = 0.5;
00205     static const double max_range = 5.5;
00206     float x_i;
00207     float y_i;
00208     float z_i;
00209     float meters;
00210     float handle_mask_width;
00211     float b_handle_x;
00212     float b_handle_y;
00213     float b_handle_z;
00214     float x; 
00215     float y;
00216     float z;
00217     float* Di;
00218     float* Da;
00219     char* Ii;
00220     cv::Point2d p_left;
00221     cv::Point2d p_up;
00222     cv::Point2d c1;
00223     cv::Point2d c2;
00224     cv::Point2d c3;
00225     cv::Point2d c4;
00226     cv::Point2d c5;
00227     cv::Point2d c6;
00228     cv::Point2d c7;
00229     cv::Point2d c8;
00230     cv::Point2d c9;
00231     cv::Point2d c10;
00232     cv::Point2d c11;
00233     cv::Point2d c12;
00234     cv::Point2d s_door_centroid;
00235     cv::Point2d s_handle;
00236     cv::Point2d s_c1;
00237     cv::Point2d s_c2;
00238     cv::Point2d s_c3;
00239     cv::Point2d s_c4;
00240     cv::Point2d b_door_centroid;
00241     cv::Point2d b_handle;
00242     cv::Point2d b_c1;
00243     cv::Point2d b_c2;
00244     cv::Point2d b_c3;
00245     cv::Point2d b_c4;
00246     cv::Vec3b color;
00247     cv::Vec3b original_color;
00248     cv::RNG rng;
00249     std::vector<float> ss_handle_x;
00250     std::vector<float> ss_handle_y;
00251     std::vector<float> ss_handle_z;
00252     std::vector<cv::Point2d> vec_up; 
00253     std::vector<cv::Point2d> vec_down;
00254     std::vector<cv::Point2d> vec_left;
00255     std::vector<cv::Point2d> vec_right;
00256     std::vector<cv::Point2d> ss_door_centroid;
00257     std::vector<cv::Point2d> ss_handle;
00258     std::vector<cv::Point2d> ss_c1;
00259     std::vector<cv::Point2d> ss_c2;
00260     std::vector<cv::Point2d> ss_c3;
00261     std::vector<cv::Point2d> ss_c4;
00262     std::vector<cv::Vec3b> colorTab;
00263     std_msgs::Int32MultiArray door_coordinates;
00264     cv_bridge::CvImagePtr depth_raw;
00265     cv_bridge::CvImagePtr canny;
00266     cv_bridge::CvImagePtr blobs;
00267     cv_bridge::CvImagePtr original;
00268     cv_bridge::CvImagePtr gradient;
00269     cv::Scalar white;
00270     cv::Scalar black;
00271     pcl::PointCloud<pcl::PointXYZ> cloud;
00272     pcl::PointXYZ point3D;
00273     pcl::PointXYZ offsetPoint3D;
00274      
00275   public:
00282     ClosedDoorDetectorAlgNode(void);
00283 
00290     ~ClosedDoorDetectorAlgNode(void);
00291 
00292   protected:
00305     void mainNodeThread(void);
00306 
00319     void node_config_update(Config &config, uint32_t level);
00320 
00327     void addNodeDiagnostics(void);
00328 
00335     void DoorSizeCalibration (cv::Mat inputImage, double min_dst_h, double max_dst_h, double min_dst_v, double max_dst_v, int handle_width, int handle_height);
00336 
00343     void DrawRect (cv::Mat inputImage, cv::Point q1, cv::Point q2, cv::Point q3, cv::Point q4, cv::Scalar color, int lineSize, int filled);
00344 
00350     double AngleBetweenLines (cv::Point point1, cv::Point point2, cv::Point point3, cv::Point point4);
00351 
00352     // [diagnostic functions]
00353     
00354     // [test functions]
00355 };
00356 
00357 #endif