00001 00060 #ifndef _PEOPLE_DETECTION_DISPLAY_ 00061 #define _PEOPLE_DETECTION_DISPLAY_ 00062 00063 // standard includes 00064 #include <sstream> 00065 #include <string> 00066 #include <vector> 00067 00068 // ROS includes 00069 #include <ros/ros.h> 00070 #include <ros/package.h> 00071 00072 // ROS message includes 00073 #include <sensor_msgs/Image.h> 00074 #include <sensor_msgs/PointCloud2.h> 00075 #include <cob_perception_msgs/DetectionArray.h> 00076 #include <cob_perception_msgs/ColorDepthImageArray.h> 00077 00078 // topics 00079 #include <message_filters/subscriber.h> 00080 #include <message_filters/synchronizer.h> 00081 #include <message_filters/sync_policies/approximate_time.h> 00082 #include <image_transport/image_transport.h> 00083 #include <image_transport/subscriber_filter.h> 00084 00085 // opencv 00086 #include <opencv2/opencv.hpp> 00087 #include <cv_bridge/cv_bridge.h> 00088 #include <sensor_msgs/image_encodings.h> 00089 00090 // boost 00091 #include <boost/bind.hpp> 00092 00093 // point cloud 00094 #include <pcl/point_types.h> 00095 #include <pcl_ros/point_cloud.h> 00096 00097 // external includes 00098 #include "cob_vision_utils/GlobalDefines.h" 00099 00100 namespace ipa_PeopleDetector 00101 { 00102 00103 class PeopleDetectionDisplayNode 00104 { 00105 protected: 00106 //message_filters::Subscriber<sensor_msgs::PointCloud2> shared_image_sub_; ///< Shared xyz image and color image topic 00107 // image_transport::SubscriberFilter people_segmentation_image_sub_; ///< Color camera image topic 00108 00109 image_transport::ImageTransport* it_; 00110 image_transport::SubscriberFilter colorimage_sub_; 00111 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, cob_perception_msgs::ColorDepthImageArray, 00112 sensor_msgs::Image> >* sync_input_3_; 00113 // message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, cob_perception_msgs::ColorDepthImageArray, sensor_msgs::PointCloud2> >* sync_input_3_; 00114 // message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub_; 00115 message_filters::Subscriber<cob_perception_msgs::ColorDepthImageArray> face_detection_subscriber_; 00116 message_filters::Subscriber<cob_perception_msgs::DetectionArray> face_recognition_subscriber_; 00117 00118 image_transport::Publisher people_detection_image_pub_; 00119 00120 ros::NodeHandle node_handle_; 00121 00122 // parameters 00123 bool display_; 00124 bool display_timing_; 00125 00126 public: 00127 00128 PeopleDetectionDisplayNode(ros::NodeHandle nh); 00129 ~PeopleDetectionDisplayNode(); 00130 00132 unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image); 00133 00135 void inputCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_recognition_msg, 00136 const cob_perception_msgs::ColorDepthImageArray::ConstPtr& face_detection_msg, const sensor_msgs::Image::ConstPtr& colorimage_msg); 00137 // void inputCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_recognition_msg, const cob_perception_msgs::ColorDepthImageArray::ConstPtr& face_detection_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg); 00138 }; 00139 00140 } 00141 ; 00142 00143 #endif // _PEOPLE_DETECTION_DISPLAY_