bow_object_detector_alg_node.h
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 _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


iri_bow_object_detector
Author(s): dmartinez
autogenerated on Fri Dec 6 2013 22:45:45