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 _iri_clothes_grasping_point_alg_node_h_
00026 #define _iri_clothes_grasping_point_alg_node_h_
00027
00028 #include <set>
00029
00030 #include <iri_base_algorithm/iri_base_algorithm.h>
00031 #include "iri_clothes_grasping_point_alg.h"
00032
00033 #include <image_transport/image_transport.h>
00034 #include <cv_bridge/cv_bridge.h>
00035 #include <sensor_msgs/image_encodings.h>
00036 #include <opencv2/imgproc/imgproc.hpp>
00037 #include <opencv2/highgui/highgui.hpp>
00038
00039
00040 #include "pcl_ros/point_cloud.h"
00041 #include "pcl/point_types.h"
00042 #include "pcl/ros/register_point_struct.h"
00043
00044
00045 #include <pcl/io/pcd_io.h>
00046
00047
00048 #include <geometry_msgs/Pose.h>
00049 #include <iri_clothes_grasping_point/GraspingPointList3d.h>
00050 #include <iri_clothes_grasping_point/GraspingPointList.h>
00051 #include <iri_clothes_grasping_point/Pixel.h>
00052
00053
00054
00055
00056
00057 typedef union {
00058 struct
00059 {
00060 unsigned char Blue;
00061 unsigned char Green;
00062 unsigned char Red;
00063 unsigned char Alpha;
00064 };
00065 float float_value;
00066 long long_value;
00067 } RGBValue;
00068
00069 static unsigned int rgb2double(int r, int g, int b){
00070 return r*255*255+g*255+b;
00071 }
00072
00073 static void double2rgb(int& r, int& g, int& b, unsigned int rgb){
00074 r = rgb/(255*255);
00075 g = (rgb/255)%255;
00076 b = (rgb%255)%255;
00077 }
00078
00083 class IriClothesGraspingPointAlgNode : public algorithm_base::IriBaseAlgorithm<IriClothesGraspingPointAlgorithm>
00084 {
00085 private:
00086 double minarea_;
00087 bool intersections_;
00088
00089 cv_bridge::CvImage cv_image;
00090
00091 int canny_threshold_;
00092 int canny_max_thresh_;
00093 void draw_contours_center(cv::Mat binary_img, cv::Mat& color_output);
00094 void draw_clothes_centers(cv::Mat& src_img);
00095 void draw_intersections(cv::Mat& src_img);
00096 void count_labels(cv::Mat labelled_image, std::set<unsigned int>& labels);
00097
00098 std::vector<cv::Point2f> grasping_point_list_;
00099
00100 cv::Mat binary_img1_;
00101 cv::Mat binary_img2_;
00102 cv::Mat intersect_;
00103 cv::Mat global_intersect_;
00104 cv::Mat global_intersect_color_;
00105 cv::Mat global_cloth_center_;
00106 cv::Mat global_cloth_center_color_;
00107
00108 void image_from_pcl2(cv::Mat& rgb_image);
00109 void image_processing(const cv::Mat src_img);
00110
00112 void binaryObjectSelection(const cv::Mat labelled_image, cv::Mat& binary_image, unsigned char Red, unsigned char Green, unsigned char Blue);
00113
00114 image_transport::ImageTransport imgtransport_;
00115
00116
00117
00118 ros::Publisher point_list_publisher_;
00119 ros::Publisher poses_list_publisher_;
00120 iri_clothes_grasping_point::GraspingPointList3d graspingPointList3d_msg_;
00121 iri_clothes_grasping_point::GraspingPointList graspingPointList_msg_;
00122 iri_clothes_grasping_point::Pixel pixel_msg_;
00123 image_transport::Publisher image_pub_;
00124 image_transport::Publisher image_pub1_;
00125 image_transport::Publisher image_pub2_;
00126 image_transport::Publisher image_pub3_;
00127 image_transport::Publisher image_pub4_;
00128 image_transport::Publisher image_pub5_;
00129 image_transport::Publisher image_pub6_;
00130 image_transport::Publisher image_pub_result_;
00131
00132
00133 image_transport::Subscriber image_sub_;
00134 ros::Subscriber pcl2_sub_;
00135 pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_;
00136
00137 void image_callback(const sensor_msgs::ImageConstPtr& msg);
00138 void pcl2_segmented_callback(const sensor_msgs::PointCloud2ConstPtr& msg);
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 public:
00155 IriClothesGraspingPointAlgNode(void);
00156
00163 ~IriClothesGraspingPointAlgNode(void);
00164
00165 protected:
00178 void mainNodeThread(void);
00179
00192 void node_config_update(Config &config, uint32_t level);
00193
00200 void addNodeDiagnostics(void);
00201
00202
00203
00204
00205 };
00206
00207 #endif