00001 #ifndef DOOR_LINE_DETECTOR 00002 #define DOOR_LINE_DETECTOR 00003 #include <ros/ros.h> 00004 #include <tf/transform_listener.h> 00005 #include "tf/message_filter.h" 00006 #include "message_filters/subscriber.h" 00007 #include <laser_geometry/laser_geometry.h> 00008 #include <visualization_msgs/Marker.h> 00009 #include <pcl/point_types.h> 00010 #include <pcl/point_cloud.h> 00011 #include <geometry_msgs/Point.h> 00012 00013 #include <door_perception_msgs/DoorState.h> 00014 #include <door_perception_msgs/DoorStateSrv.h> 00015 00016 typedef pcl::PointXYZ PointType; 00017 typedef pcl::PointCloud<PointType> PointCloudType; 00019 00023 class DoorAngleDetector{ 00024 public: 00025 DoorAngleDetector(); 00026 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan); 00027 bool serviceCallback(door_perception_msgs::DoorStateSrv::Request& request, door_perception_msgs::DoorStateSrv::Response& response); 00028 00030 void estimateDoorState(const PointCloudType& pcl_cloud , double& angle_out, geometry_msgs::Point& axis_position_out); 00031 private: 00032 ros::NodeHandle node_; 00033 tf::TransformListener tfListener_; 00034 laser_geometry::LaserProjection projector_; 00035 00036 message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_; 00037 ros::Publisher point_cloud_publisher_; 00038 ros::Publisher door_state_publisher_; 00039 ros::Publisher vis_pub_; 00040 ros::Publisher angle_pub_; 00041 std::string door_frame_; 00042 tf::MessageFilter<sensor_msgs::LaserScan>* tf_filter_; 00043 PointCloudType cropping_polygon_; 00044 ros::ServiceServer service; 00045 sensor_msgs::LaserScan last_scan_; 00046 double slope, intercept; //Only interesting for axis computation 00047 double angle; 00048 bool door_found; 00049 }; 00050 00052 visualization_msgs::Marker visualize_door(geometry_msgs::Point p1, geometry_msgs::Point p2, std_msgs::Header header); 00053 00054 #endif
