Go to the documentation of this file.00001 #ifndef DOOR_PLANE_DETECTOR
00002 #define DOOR_PLANE_DETECTOR
00003 #include <ros/ros.h>
00004 #include <tf/transform_listener.h>
00005 #include <tf/transform_broadcaster.h>
00006 #include "tf/message_filter.h"
00007 #include "message_filters/subscriber.h"
00008 #include <pcl/point_types.h>
00009
00010 #include <pcl_ros/point_cloud.h>
00011 #include <Eigen/Core>
00012 #include <std_srvs/Empty.h>
00013
00014 #include <door_perception_msgs/DoorState.h>
00015 #include <door_perception_msgs/DoorStateSrv.h>
00016
00017 typedef pcl::PointXYZ PointType;
00018 typedef pcl::PointCloud<PointType> PointCloudType;
00019
00021
00025 class DoorHandleDetector{
00026 public:
00027 DoorHandleDetector();
00028 void cloudCallback(const PointCloudType::ConstPtr& msg);
00029 bool serviceCallback(door_perception_msgs::DoorStateSrv::Request& request, door_perception_msgs::DoorStateSrv::Response& response);
00030
00032 void estimateDoorHandle(const PointCloudType& cloud , PointCloudType& handle_cloud);
00033 bool doorHandleRansac(const PointCloudType& cloud, PointCloudType& handle_cloud, Eigen::Vector3f& door_normal, double& plane_distance_to_origin );
00034 bool switchActive(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res );
00035 bool switchInactive(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res );
00036 private:
00037 ros::NodeHandle node_;
00038 tf::TransformListener tfListener_;
00039
00040 message_filters::Subscriber<PointCloudType> cloud_sub_;
00041 ros::Publisher door_cloud_publisher_;
00042 ros::Publisher handle_cloud_publisher_;
00043 ros::Publisher door_state_publisher_;
00044 ros::Publisher vis_pub_;
00045 ros::Publisher handle_pose_pub_;
00046 ros::ServiceServer service_on_;
00047 ros::ServiceServer service_off_;
00048 ros::ServiceServer door_state_service_;
00049 std::string door_frame_;
00050 tf::MessageFilter<PointCloudType>* tf_filter_;
00051 PointCloudType cropping_polygon_;
00052 tf::TransformBroadcaster br_;
00053 bool active_;
00054 std::string filter_polygon_filename_;
00055 double door_radius_;
00056 door_perception_msgs::DoorState door_state_msg_;
00057 };
00058 bool computeMainAxis(const PointCloudType& pc, Eigen::Vector3f& mean, Eigen::Vector3f& main_axis, float eval_ratio_threshold);
00059 #endif
