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 <opencv/cv.h> 00087 #include <opencv/highgui.h> 00088 #include <cv_bridge/cv_bridge.h> 00089 #include <sensor_msgs/image_encodings.h> 00090 00091 // boost 00092 #include <boost/bind.hpp> 00093 00094 // point cloud 00095 #include <pcl/point_types.h> 00096 #include <pcl_ros/point_cloud.h> 00097 00098 // external includes 00099 #include "cob_vision_utils/GlobalDefines.h" 00100 00101 namespace ipa_PeopleDetector 00102 { 00103 00104 class PeopleDetectionDisplayNode 00105 { 00106 protected: 00107 //message_filters::Subscriber<sensor_msgs::PointCloud2> shared_image_sub_; ///< Shared xyz image and color image topic 00108 // image_transport::SubscriberFilter people_segmentation_image_sub_; ///< Color camera image topic 00109 00110 image_transport::ImageTransport* it_; 00111 image_transport::SubscriberFilter colorimage_sub_; 00112 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, cob_perception_msgs::ColorDepthImageArray, 00113 sensor_msgs::Image> >* sync_input_3_; 00114 // message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, cob_perception_msgs::ColorDepthImageArray, sensor_msgs::PointCloud2> >* sync_input_3_; 00115 // message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub_; 00116 message_filters::Subscriber<cob_perception_msgs::ColorDepthImageArray> face_detection_subscriber_; 00117 message_filters::Subscriber<cob_perception_msgs::DetectionArray> face_recognition_subscriber_; 00118 00119 image_transport::Publisher people_detection_image_pub_; 00120 00121 ros::NodeHandle node_handle_; 00122 00123 // parameters 00124 bool display_; 00125 bool display_timing_; 00126 00127 public: 00128 00129 PeopleDetectionDisplayNode(ros::NodeHandle nh); 00130 ~PeopleDetectionDisplayNode(); 00131 00133 unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image); 00134 00136 void inputCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_recognition_msg, 00137 const cob_perception_msgs::ColorDepthImageArray::ConstPtr& face_detection_msg, const sensor_msgs::Image::ConstPtr& colorimage_msg); 00138 // 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); 00139 }; 00140 00141 } 00142 ; 00143 00144 #endif // _PEOPLE_DETECTION_DISPLAY_