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 _bow_object_detector_alg_node_h_ 00026 #define _bow_object_detector_alg_node_h_ 00027 00028 #include <iri_base_algorithm/iri_base_algorithm.h> 00029 #include "bow_object_detector_alg.h" 00030 00031 // [publisher subscriber headers] 00032 #include <normal_descriptor_node/ndesc_pc.h> 00033 #include <iri_bow_object_detector/WrinkledMap.h> 00034 #include <sensor_msgs/Image.h> 00035 #include <sensor_msgs/PointCloud2.h> 00036 00037 // [service client headers] 00038 #include <iri_perception_msgs/PclToDescriptorSet.h> 00039 #include <iri_perception_msgs/DescriptorsToVws.h> 00040 #include <iri_sift/DescriptorsFromImage.h> 00041 #include <iri_perception_msgs/SetImage.h> 00042 #include <iri_bow_object_detector/RefineGraspPoint.h> 00043 #include <iri_bow_object_detector/GeoVwDetection.h> 00044 #include <iri_sift/GeoVwSetSrv.h> 00045 00046 // [action server client headers] 00047 #include <iri_action_server/iri_action_server.h> 00048 #include <iri_bow_object_detector/GetGraspingPointAction.h> 00049 00050 //OpenCV 00051 #include <cv_bridge/cv_bridge.h> 00052 #include <sensor_msgs/image_encodings.h> 00053 #include <opencv2/imgproc/imgproc.hpp> 00054 00055 00056 // PCL 00057 #include <pcl/point_cloud.h> 00058 #include <pcl/io/pcd_io.h> 00059 #include <pcl/filters/passthrough.h> 00060 #include <pcl/segmentation/extract_clusters.h> 00061 #include <pcl_conversions/pcl_conversions.h> 00062 // events 00063 #include "eventexceptions.h" 00064 #include "event.h" 00065 00070 class BowObjectDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<BowObjectDetectorAlgorithm> 00071 { 00072 private: 00073 // [publisher attributes] 00074 ros::Publisher image_out_publisher_; 00075 sensor_msgs::Image Image_msg_; 00076 ros::Publisher pointcloud_out_publisher_; 00077 sensor_msgs::PointCloud2 PointCloud2_msg_; 00078 00079 // [subscriber attributes] 00080 ros::Subscriber mask_image_in_subscriber_; 00081 void mask_image_in_callback(const sensor_msgs::Image::ConstPtr& msg); 00082 CMutex mask_image_in_mutex_; 00083 ros::Subscriber pointcloud_in_subscriber_; 00084 void pointcloud_in_callback(const sensor_msgs::PointCloud2::ConstPtr& msg); 00085 CMutex pointcloud_in_mutex_; 00086 00087 // [service attributes] 00088 00089 // [client attributes] 00090 ros::ServiceClient pointcloud_to_descriptorset_client_; 00091 iri_perception_msgs::PclToDescriptorSet pointcloud_to_descriptorset_srv_; 00092 ros::ServiceClient get_vws_client_; 00093 iri_perception_msgs::DescriptorsToVws get_vws_srv_; 00094 ros::ServiceClient get_sift_descriptors_client_; 00095 iri_sift::DescriptorsFromImage get_sift_descriptors_srv_; 00096 ros::ServiceClient set_background_image_client_; 00097 iri_perception_msgs::SetImage set_background_image_srv_; 00098 ros::ServiceClient select_grasp_point_client_; 00099 iri_bow_object_detector::RefineGraspPoint select_grasp_point_srv_; 00100 ros::ServiceClient detectObjects_client_; 00101 iri_bow_object_detector::GeoVwDetection detectObjects_srv_; 00102 ros::ServiceClient getVwSet_client_; 00103 iri_sift::GeoVwSetSrv getVwSet_srv_; 00104 00105 // [action server attributes] 00106 IriActionServer<iri_bow_object_detector::GetGraspingPointAction> get_grasping_point_aserver_; 00107 void get_grasping_pointStartCallback(const iri_bow_object_detector::GetGraspingPointGoalConstPtr& goal); 00108 void get_grasping_pointStopCallback(void); 00109 bool get_grasping_pointIsFinishedCallback(void); 00110 bool get_grasping_pointHasSucceedCallback(void); 00111 void get_grasping_pointGetResultCallback(iri_bow_object_detector::GetGraspingPointResultPtr& result); 00112 void get_grasping_pointGetFeedbackCallback(iri_bow_object_detector::GetGraspingPointFeedbackPtr& feedback); 00113 00114 // [action client attributes] 00115 00116 // other 00117 CEvent EventPointCloudReady, EventImageMaskReady, EventWrinkledMapReady; 00118 00119 iri_perception_msgs::DescriptorSet last_sift_descriptors_; 00120 iri_perception_msgs::GeoVwSet last_sift_vw_set_; 00121 sensor_msgs::PointCloud2 last_pointcloud_; 00122 sensor_msgs::Image last_image_mask_; 00123 sensor_msgs::Image last_image_; 00124 //iri_bow_object_detector::WrinkledMap last_wrinkled_map_; 00125 sensor_msgs::PointCloud2::ConstPtr last_wrinkled_map_; 00126 normal_descriptor_node::ndesc_pc::ConstPtr last_normal_descriptor_; 00127 00128 bool first_pointcloud_; 00129 bool pointcloud_ready_; 00130 bool image_mask_ready_; 00131 bool wrinkled_map_ready_; 00132 bool solution_ready_; 00133 00134 sensor_msgs::Image ros_pointcloud_to_ros_image(sensor_msgs::PointCloud2 ros_cloud); 00135 cv::Mat pcl_pointcloud_to_cv_image(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud); 00136 sensor_msgs::ImagePtr cv_image_to_ros_image(cv::Mat cvimage, std_msgs::Header header); 00137 cv::Mat crop_and_resize(cv::Mat& image); 00138 void save_pcd_to_disk(sensor_msgs::PointCloud2 ros_cloud); 00139 00140 void detectGraspPoint(); 00141 void training(); 00142 00143 00144 public: 00151 BowObjectDetectorAlgNode(void); 00152 00159 ~BowObjectDetectorAlgNode(void); 00160 00161 protected: 00174 void mainNodeThread(void); 00175 00188 void node_config_update(Config &config, uint32_t level); 00189 00196 void addNodeDiagnostics(void); 00197 00198 // [diagnostic functions] 00199 00200 // [test functions] 00201 }; 00202 00203 #endif