door_cloud_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 _door_cloud_alg_node_h_
00026 #define _door_cloud_alg_node_h_
00027 
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "door_cloud_alg.h"
00030 #include "boost/tuple/tuple.hpp"
00031 
00032 // tf
00033 #include <tf/transform_broadcaster.h>
00034 #include <tf/transform_datatypes.h>
00035 
00036 // PCL 
00037 #include <pcl/ros/conversions.h>
00038 #include <pcl/point_cloud.h>
00039 #include <pcl/point_types.h>
00040 #include <pcl/ModelCoefficients.h>
00041 #include <pcl/sample_consensus/method_types.h>
00042 #include <pcl/sample_consensus/model_types.h>
00043 #include <pcl/segmentation/sac_segmentation.h>
00044 #include <pcl/filters/extract_indices.h>
00045 #include <pcl/filters/passthrough.h>
00046 #include <pcl/filters/voxel_grid.h>
00047 #include <pcl/common/transform.h>
00048 #include <pcl/registration/transformation_estimation_svd.h>
00049 
00050 // [publisher subscriber headers]
00051 #include <std_msgs/Int8.h>
00052 #include <visualization_msgs/Marker.h>
00053 #include <sensor_msgs/PointCloud2.h>
00054 #include <geometry_msgs/PoseStamped.h>
00055 
00056 // [service client headers]
00057 
00058 // [action server client headers]
00059 
00064 class DoorCloudAlgNode : public algorithm_base::IriBaseAlgorithm<DoorCloudAlgorithm>
00065 {
00066   private:
00067     // [publisher attributes]
00068     ros::Publisher open_door_cloud_publisher_;
00069     sensor_msgs::PointCloud2 PointCloud2_msg_;
00070     ros::Publisher open_door_marker_publisher_;
00071     visualization_msgs::Marker Marker_msg_;
00072     ros::Publisher door_centroid_publisher_;
00073     geometry_msgs::PoseStamped PoseStamped_msg_;
00074     ros::Publisher closed_door_cloud_publisher_;
00075     //sensor_msgs::PointCloud2 PointCloud2_msg_;
00076     ros::Publisher closed_door_marker_publisher_;
00077     //visualization_msgs::Marker Marker_msg_;
00078     ros::Publisher door_handle_publisher_;
00079     //geometry_msgs::PoseStamped PoseStamped_msg_;
00080     
00081 
00082     // [subscriber attributes]
00083     ros::Subscriber door_action_start_subscriber_;
00084     void door_action_start_callback(const std_msgs::Int8::ConstPtr& msg);
00085     CMutex door_action_start_mutex_;
00086     ros::Subscriber points_subscriber_;
00087     void points_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00088     CMutex points_mutex_;
00089     ros::Subscriber door_centroid_subscriber_;
00090     void door_centroid_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
00091     CMutex door_centroid_mutex_;
00092     ros::Subscriber handle_location_subscriber_;
00093     void handle_location_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
00094     CMutex handle_location_mutex_;
00095 
00096     // [service attributes]
00097 
00098     // [client attributes]
00099 
00100     // [action server attributes]
00101 
00102     // [action client attributes]
00103 
00104     // [reconfigurable variables]
00105     bool no_simulator;
00106 
00107     // [algorithm variables]
00108     int captured_open;
00109     int captured_closed;
00110     int start;
00111     float x_left;
00112     float x_right;
00113     float x_open;
00114     float y_top;
00115     float y_open;
00116     float z_open;
00117     float w_open;
00118     float x_closed;
00119     float y_closed; 
00120     float z_closed;
00121     float w_closed;
00122     float lower_x;
00123     float lower_y;
00124     float lower_z;
00125     float upper_x;
00126     float upper_y;
00127     float upper_z;
00128     std::string tf_original_frame;
00129     pcl::PointCloud<pcl::PointXYZ> cloud;
00130     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
00131     pcl::PointCloud<pcl::PointXYZ> cloud_filtered_left;
00132     pcl::PointCloud<pcl::PointXYZ> cloud_filtered_right;
00133     pcl::PointCloud<pcl::PointXYZ> cloud_filtered_top;
00134     pcl::PointCloud<pcl::PointXYZ> cloud_plane; 
00135     boost::tuple<pcl::PointCloud<pcl::PointXYZ>, pcl::PointIndices::Ptr, pcl::ModelCoefficients::Ptr> door_plane;
00136     geometry_msgs::PoseStamped poses;
00137     geometry_msgs::Quaternion orient;
00138     sensor_msgs::PointCloud2 open_door_cloud;
00139     sensor_msgs::PointCloud2 closed_door_cloud;
00140     visualization_msgs::Marker marker;
00141 
00142   public:
00149     DoorCloudAlgNode(void);
00150 
00157     ~DoorCloudAlgNode(void);
00158 
00159   protected:
00172     void mainNodeThread(void);
00173 
00186     void node_config_update(Config &config, uint32_t level);
00187 
00194     void addNodeDiagnostics(void);
00195 
00201     pcl::PointCloud<pcl::PointXYZ> filterCloud (pcl::PointCloud<pcl::PointXYZ> raw_cloud, float Lowx, float Uppx, float Lowy, float Uppy, float Lowz, float Uppz, bool negative_limits);
00202 
00208     boost::tuple<pcl::PointCloud<pcl::PointXYZ>, pcl::PointIndices::Ptr,pcl::ModelCoefficients::Ptr> planeFit (pcl::PointCloud<pcl::PointXYZ> raw_cloud);
00209 
00215     geometry_msgs::Quaternion quaternionFromVectors (tf::Vector3 vector_1, tf::Vector3 vector_2);
00216 
00222     visualization_msgs::Marker ArrowMarker(std_msgs::Header header, geometry_msgs::Pose pose, int alpha, int color,  const char arrow_tag []);
00223 
00224     // [diagnostic functions]
00225     
00226     // [test functions]
00227 };
00228 
00229 #endif


iri_door_detector
Author(s): Jose Rodriguez
autogenerated on Fri Dec 6 2013 23:57:16