00001 00060 #ifndef __SENSOR_MESSAGE_GATEWAY_NODE_H__ 00061 #define __SENSOR_MESSAGE_GATEWAY_NODE_H__ 00062 00063 #ifdef __LINUX__ 00064 #else 00065 #endif 00066 00067 // ROS includes 00068 #include <ros/ros.h> 00069 #include <ros/package.h> // use as: directory_ = ros::package::getPath("cob_people_detection") + "/common/files/windows/"; 00070 // ROS message includes 00071 #include <sensor_msgs/Image.h> 00072 #include <sensor_msgs/PointCloud2.h> 00073 00074 // image transport 00075 #include <image_transport/image_transport.h> 00076 #include <image_transport/subscriber_filter.h> 00077 00078 // dynamic reconfigure 00079 #include <dynamic_reconfigure/server.h> 00080 #include <cob_people_detection/sensor_message_gatewayConfig.h> 00081 00082 // boost 00083 #include <boost/thread/mutex.hpp> 00084 00085 namespace cob_people_detection 00086 { 00087 class SensorMessageGatewayNode 00088 { 00089 public: 00090 00093 SensorMessageGatewayNode(ros::NodeHandle nh); 00094 ~SensorMessageGatewayNode(void); 00095 00096 protected: 00097 00099 void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& pointcloud); 00100 00101 void imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg); 00102 00103 void reconfigureCallback(cob_people_detection::sensor_message_gatewayConfig &config, uint32_t level); 00104 00105 dynamic_reconfigure::Server<cob_people_detection::sensor_message_gatewayConfig> reconfigure_server_; 00106 00107 ros::NodeHandle node_handle_; 00108 00109 ros::Subscriber pointcloud_sub_; 00110 ros::Publisher pointcloud_pub_; 00111 00112 image_transport::ImageTransport* it_; 00113 image_transport::SubscriberFilter color_image_sub_; 00114 image_transport::Publisher color_image_pub_; 00115 00116 ros::Duration target_publishing_delay_; 00117 00118 // image message buffer 00119 sensor_msgs::Image image_buffer_; // stores the received color image until the corresponding pointcloud is received and published 00120 00121 // mutex 00122 boost::mutex image_buffer_mutex_; 00123 00124 // parameters 00125 double target_publishing_rate_; 00126 ros::Time last_publishing_time_pcl_; 00127 ros::Time last_publishing_time_image_; 00128 bool display_timing_; 00129 }; 00130 00131 } 00132 00133 #endif // __SENSOR_MESSAGE_GATEWAY_NODE_H__