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
00026
00027
00028
00029
00030
00046 #ifndef DOORS_DETECTOR_H
00047 #define DOORS_DETECTOR_H
00048
00049
00050 #include <ros/ros.h>
00051 #include <roslib/Header.h>
00052 #include <mapping_msgs/PolygonalMap.h>
00053
00054
00055 #include <door_handle_detector/geometric_helper.h>
00056
00057
00058 #include <door_handle_detector/DoorsDetector.h>
00059 #include <door_handle_detector/DoorsDetectorCloud.h>
00060
00061 #include <angles/angles.h>
00062
00063
00064 namespace door_handle_detector{
00065
00066 class DoorDetector
00067 {
00068
00069 public:
00070 DoorDetector ();
00071 ~DoorDetector (){};
00072
00074 bool detectDoorSrv (door_handle_detector::DoorsDetector::Request &req,
00075 door_handle_detector::DoorsDetector::Response &resp);
00077 bool detectDoorCloudSrv (door_handle_detector::DoorsDetectorCloud::Request &req,
00078 door_handle_detector::DoorsDetectorCloud::Response &resp);
00079
00080 ros::ServiceServer detect_srv_, detect_cloud_srv_;
00081 ros::Publisher viz_marker_pub_, door_frames_pub_, door_regions_pub_;
00082
00083 private:
00085 bool detectDoors(const door_msgs::Door& door, sensor_msgs::PointCloud pointcloud,
00086 std::vector<door_msgs::Door>& result) const;
00087
00089 void cloud_cb (const sensor_msgs::PointCloudConstPtr& cloud);
00090
00091 double distToHinge(const door_msgs::Door& door, geometry_msgs::Point32& pnt) const;
00092
00093 ros::NodeHandle node_tilde_, node_;
00094 mutable int global_marker_id_;
00095
00096
00097 sensor_msgs::PointCloud pointcloud_;
00098 std::string input_cloud_topic_;
00099 unsigned int num_clouds_received_;
00100 geometry_msgs::Point32 z_axis_;
00101 tf::TransformListener tf_;
00102
00103 std::string parameter_frame_, fixed_frame_;
00104
00105
00106 double door_min_height_, door_min_width_, door_max_height_, door_max_width_, door_min_z_, max_dist_from_prior_;
00107
00108
00109 double leaf_width_;
00110 double sac_distance_threshold_;
00111 double normal_angle_tolerance_;
00112 int k_search_;
00113
00114
00115 double euclidean_cluster_angle_tolerance_, euclidean_cluster_distance_tolerance_;
00116 int euclidean_cluster_min_pts_;
00117
00118
00119 double rectangle_constrain_edge_height_;
00120 double rectangle_constrain_edge_angle_;
00121
00122
00123 double minimum_region_density_;
00124 double maximum_search_radius_, maximum_search_radius_limit_, maximum_scan_angle_limit_;
00125 double minimum_z_, maximum_z_;;
00126
00127
00128
00129
00130 };
00131
00132
00133 };
00134
00135 #endif