00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. 00002 // Author 00003 // All rights reserved. 00004 // 00005 // This file is part of iri-ros-pkg 00006 // iri-ros-pkg is free software: you can redistribute it and/or modify 00007 // it under the terms of the GNU Lesser General Public License as published by 00008 // the Free Software Foundation, either version 3 of the License, or 00009 // at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU Lesser General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU Lesser General Public License 00017 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00018 // 00019 // IMPORTANT NOTE: This code has been generated through a script from the 00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness 00021 // of the scripts. ROS topics can be easly add by using those scripts. Please 00022 // refer to the IRI wiki page for more information: 00023 // http://wikiri.upc.es/index.php/Robotics_Lab 00024 00025 #ifndef _camera_people_detection_alg_node_h_ 00026 #define _camera_people_detection_alg_node_h_ 00027 00028 //std headers 00029 //#include <vector> 00030 00031 //IRI-ROS 00032 #include <iri_base_algorithm/iri_base_algorithm.h> 00033 #include "camera_people_detection_alg.h" 00034 00035 //IRI external library 00036 #include "camera_people_detector.h" 00037 00038 //required headers for image I/O 00039 //#include <ros/ros.h> 00040 #include <image_transport/image_transport.h> 00041 #include <cv_bridge/cv_bridge.h> 00042 #include <sensor_msgs/image_encodings.h> 00043 #include <opencv2/imgproc/imgproc.hpp> 00044 #include <opencv2/highgui/highgui.hpp> 00045 00046 //other ros stuff 00047 #include <tf/tfMessage.h> 00048 #include <tf/transform_broadcaster.h> 00049 #include <tf/transform_listener.h> 00050 #include <image_geometry/pinhole_camera_model.h> 00051 00052 // [publisher subscriber headers] 00053 #include <iri_perception_msgs/peopleTrackingArray.h> 00054 #include <sensor_msgs/LaserScan.h> 00055 #include <iri_perception_msgs/detectionArray.h> 00056 #include <visualization_msgs/MarkerArray.h> 00057 00058 // [service client headers] 00059 00060 // [action server client headers] 00061 00062 #include "eventserver.h" 00063 00068 class CameraPeopleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<CameraPeopleDetectionAlgorithm> 00069 { 00070 private: 00071 // [publisher attributes] 00072 ros::Publisher markers_publisher_; 00073 ros::Publisher image_detection_publisher_; 00074 image_transport::Publisher debug_img_publisher_; 00075 00076 // [subscriber attributes] 00077 ros::Subscriber tracker_subscriber_; 00078 void tracker_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg); 00079 CMutex tracker_mutex_; 00080 00081 ros::Subscriber laser_subscriber_; 00082 void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg); 00083 CMutex laser_mutex_; 00084 00085 ros::Subscriber people_detection_subscriber_; 00086 void people_detection_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg); 00087 CMutex people_detection_mutex_; 00088 00089 image_transport::ImageTransport it_; 00090 image_transport::CameraSubscriber cam_sub_; 00091 image_geometry::PinholeCameraModel cam_model_; 00092 void image_callback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); 00093 CMutex image_mutex_; 00094 00095 // [service attributes] 00096 00097 // [client attributes] 00098 00099 // [action server attributes] 00100 00101 // [action client attributes] 00102 00103 //class members 00104 cv_bridge::CvImagePtr cv_ptr_; 00105 CCamera_People_Detector cam_detector_; 00106 sensor_msgs::LaserScan scan_; 00107 tf::TransformListener tf_listener_; 00108 iri_perception_msgs::detectionArray laser_poses_msg_; 00109 void filterLaserDetections(iri_perception_msgs::detectionArray & fusion_poses_msg, 00110 cv_bridge::CvImage & img_msg); 00111 void fillMarkerArray(const iri_perception_msgs::detectionArray & fusion_poses_msg, 00112 visualization_msgs::MarkerArray & markerArray_msg); 00113 CEventServer * event_server_; 00114 std::string new_laser_event_id_; 00115 std::string new_image_event_id_; 00116 std::string new_tracker_event_id_; 00117 00118 iri_perception_msgs::peopleTrackingArray tracker_msg_; 00119 00120 float width_resize_; 00121 float height_resize_; 00122 double minthr_; 00123 00124 public: 00131 CameraPeopleDetectionAlgNode(void); 00132 00139 ~CameraPeopleDetectionAlgNode(void); 00140 00141 protected: 00154 void mainNodeThread(void); 00155 00168 void node_config_update(Config &config, uint32_t level); 00169 00176 void addNodeDiagnostics(void); 00177 00178 // [diagnostic functions] 00179 00180 // [test functions] 00181 }; 00182 00183 #endif