00001 /* 00002 * Copyright 2015 Aldebaran 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 * 00016 */ 00017 00018 #ifndef PUBLISHER_CAMERA_HPP 00019 #define PUBLISHER_CAMERA_HPP 00020 00021 /* 00022 * ROS includes 00023 */ 00024 #include <ros/ros.h> 00025 #include <image_transport/image_transport.h> 00026 00027 namespace naoqi 00028 { 00029 namespace publisher 00030 { 00031 00032 class CameraPublisher 00033 { 00034 public: 00035 CameraPublisher( const std::string& topic, int camera_source ); 00036 00037 ~CameraPublisher(); 00038 00039 inline std::string topic() const 00040 { 00041 return topic_; 00042 } 00043 00044 inline bool isInitialized() const 00045 { 00046 return is_initialized_; 00047 } 00048 00049 void publish( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info ); 00050 00051 void reset( ros::NodeHandle& nh ); 00052 00053 inline bool isSubscribed() const 00054 { 00055 if (is_initialized_ == false) return false; 00056 return pub_.getNumSubscribers() > 0; 00057 } 00058 00059 private: 00060 std::string topic_; 00061 00062 bool is_initialized_; 00063 00064 //image_transport::ImageTransport it_; 00065 image_transport::CameraPublisher pub_; 00066 00067 int camera_source_; 00068 }; 00069 00070 } //publisher 00071 } //naoqi 00072 00073 00074 #endif