Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _open_door_detector_alg_node_h_
00026 #define _open_door_detector_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "open_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_listener.h>
00038 #include <tf/transform_datatypes.h>
00039 #include <algorithm>
00040
00041
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
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
00061
00062
00063
00064
00069 class OpenDoorDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<OpenDoorDetectorAlgorithm>
00070 {
00071 private:
00072
00073 ros::Publisher door_centroid_publisher_;
00074 geometry_msgs::PoseStamped PoseStamped_msg_;
00075 ros::Publisher open_door_coordinates_publisher_;
00076 std_msgs::Int32MultiArray Int32MultiArray_msg_;
00077
00078
00079 ros::Subscriber door_action_start_subscriber_;
00080 void door_action_start_callback(const std_msgs::Int8::ConstPtr& msg);
00081 CMutex door_action_start_mutex_;
00082 ros::Subscriber image_color_subscriber_;
00083 void image_color_callback(const sensor_msgs::Image::ConstPtr& msg);
00084 CMutex image_color_mutex_;
00085 ros::Subscriber points_subscriber_;
00086 void points_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00087 CMutex points_mutex_;
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098 int min_x;
00099 int max_x;
00100 int min_y;
00101 int max_y;
00102 int debugging_images;
00103 bool no_simulator;
00104 bool DSC;
00105 bool SVP;
00106 bool SVL;
00107 bool SFC;
00108 bool Range_filter;
00109 bool Persp_filter;
00110 bool Geom_filter;
00111 bool Aspect_filter;
00112 bool Size_filter;
00113 double security_distance;
00114
00115
00116 int idx;
00117 int idx_left;
00118 int idx_right;
00119 int idx_top;
00120 int point;
00121 int count;
00122 int frame_x;
00123 int frame_y;
00124 int cx_min;
00125 int cx_max;
00126 int cy_min;
00127 int cy_max;
00128 int roi_x;
00129 int roi_y;
00130 int door_x;
00131 int door_y;
00132 int p_set;
00133 int f_c1;
00134 int f_c2;
00135 int f_c3;
00136 int f_c4;
00137 int offset_l;
00138 int offset_r;
00139 int offset_u;
00140 int offset_d;
00141 int check;
00142 int aspect;
00143 int perspective;
00144 int geometry;
00145 int size;
00146 int sensor_range;
00147 int start;
00148 int captured_depth;
00149 int support;
00150 int sill;
00151 int lintel;
00152 int room_frame_height;
00153 int jamb;
00154 int left_door_end;
00155 int right_door_end;
00156 int door_end;
00157 size_t vec_start;
00158 size_t vec_end;
00159 double step_x;
00160 double step_y;
00161 double m1;
00162 double m2;
00163 double b1;
00164 double b2;
00165 double dx;
00166 double dy;
00167 double dst_h;
00168 double dst_v;
00169 double min_dst_h;
00170 double min_dst_v;
00171 double max_dst_v;
00172 double max_dst_h;
00173 double dst_up;
00174 double dst_down;
00175 double dst_left;
00176 double dst_right;
00177 double theta;
00178 double theta1;
00179 double theta2;
00180 double theta3;
00181 double theta4;
00182 double r;
00183 double dst;
00184 double min_range;
00185 double max_range;
00186 float area_ratio;
00187 float frame_left;
00188 float frame_right;
00189 float frame_up;
00190 float left_door_edge;
00191 float right_door_edge;
00192 float prev_depth;
00193 float min_depth;
00194 float max_depth;
00195 float meters;
00196 float side;
00197 float x_i;
00198 float x_left;
00199 float x_right;
00200 float y_i;
00201 float y_top;
00202 float z_i;
00203 float x_1;
00204 float x_2;
00205 float y_2;
00206 float y_1;
00207 float x;
00208 float y;
00209 float z;
00210 float distance_to_room;
00211 float room_door_ratio;
00212 float left_depth;
00213 float right_depth;
00214 float b_depth;
00215 float* Dii;
00216 float* Di;
00217 float* Da;
00218 char* Ii;
00219 cv::Point2d s_door_centroid;
00220 cv::Point2d c1;
00221 cv::Point2d c2;
00222 cv::Point2d c3;
00223 cv::Point2d c4;
00224 cv::Point2d c5;
00225 cv::Point2d c6;
00226 cv::Point2d c7;
00227 cv::Point2d c8;
00228 cv::Point2d b_door_centroid;
00229 cv::Point2d b_handle;
00230 cv::Point2d b_c1;
00231 cv::Point2d b_c2;
00232 cv::Point2d b_c3;
00233 cv::Point2d b_c4;
00234 cv::RNG rng;
00235 cv_bridge::CvImagePtr depth_raw;
00236 cv_bridge::CvImagePtr original;
00237 std::vector<cv::Point2d> vec_left;
00238 std::vector<cv::Point2d> vec_right;
00239 std::vector<cv::Point2d> vec_up;
00240 std::vector<cv::Point2d> vec_down;
00241 std::vector<cv::Point2d> ss_door_centroid;
00242 std::vector<float> ss_depth;
00243 std::vector<cv::Point2d> ss_c1;
00244 std::vector<cv::Point2d> ss_c2;
00245 std::vector<cv::Point2d> ss_c3;
00246 std::vector<cv::Point2d> ss_c4;
00247 std::vector<cv::Vec3b> colorTab;
00248 geometry_msgs::PoseStamped poses;
00249 std_msgs::Int32MultiArray door_coordinates;
00250 cv::Scalar white;
00251 cv::Scalar black;
00252 pcl::PointCloud<pcl::PointXYZ> cloud;
00253 pcl::PointXYZ point3D;
00254
00255
00256 public:
00263 OpenDoorDetectorAlgNode(void);
00264
00271 ~OpenDoorDetectorAlgNode(void);
00272
00273 protected:
00286 void mainNodeThread(void);
00287
00300 void node_config_update(Config &config, uint32_t level);
00301
00308 void addNodeDiagnostics(void);
00309
00316 void DoorSizeCalibration (cv::Mat inputImage, double min_dst_h, double max_dst_h, double min_dst_v, double max_dst_v);
00317
00324 void DrawRect (cv::Mat inputImage, cv::Point q1, cv::Point q2, cv::Point q3, cv::Point q4, cv::Scalar color, int lineSize, int filled);
00325
00331 double AngleBetweenLines (cv::Point point1, cv::Point point2, cv::Point point3, cv::Point point4);
00332
00333
00334
00335
00336 };
00337
00338 #endif