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 _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
00033 #include <tf/transform_broadcaster.h>
00034 #include <tf/transform_datatypes.h>
00035
00036
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
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
00057
00058
00059
00064 class DoorCloudAlgNode : public algorithm_base::IriBaseAlgorithm<DoorCloudAlgorithm>
00065 {
00066 private:
00067
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
00076 ros::Publisher closed_door_marker_publisher_;
00077
00078 ros::Publisher door_handle_publisher_;
00079
00080
00081
00082
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
00097
00098
00099
00100
00101
00102
00103
00104
00105 bool no_simulator;
00106
00107
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
00225
00226
00227 };
00228
00229 #endif