camera_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "image_transport/image_transport.h"
00036 #include "image_transport/camera_common.h"
00037 
00038 namespace image_transport {
00039 
00040 struct CameraPublisher::Impl
00041 {
00042   Impl()
00043     : unadvertised_(false)
00044   {
00045   }
00046 
00047   ~Impl()
00048   {
00049     shutdown();
00050   }
00051 
00052   bool isValid() const
00053   {
00054     return !unadvertised_;
00055   }
00056   
00057   void shutdown()
00058   {
00059     if (!unadvertised_) {
00060       unadvertised_ = true;
00061       image_pub_.shutdown();
00062       info_pub_.shutdown();
00063     }
00064   }
00065 
00066   Publisher image_pub_;
00067   ros::Publisher info_pub_;
00068   bool unadvertised_;
00069   //double constructed_;
00070 };
00071 
00072 CameraPublisher::CameraPublisher(ImageTransport& image_it, ros::NodeHandle& info_nh,
00073                                  const std::string& base_topic, uint32_t queue_size,
00074                                  const SubscriberStatusCallback& image_connect_cb,
00075                                  const SubscriberStatusCallback& image_disconnect_cb,
00076                                  const ros::SubscriberStatusCallback& info_connect_cb,
00077                                  const ros::SubscriberStatusCallback& info_disconnect_cb,
00078                                  const ros::VoidPtr& tracked_object, bool latch)
00079   : impl_(new Impl)
00080 {
00081   // Explicitly resolve name here so we compute the correct CameraInfo topic when the
00082   // image topic is remapped (#4539).
00083   std::string image_topic = info_nh.resolveName(base_topic);
00084   std::string info_topic = getCameraInfoTopic(image_topic);
00085 
00086   impl_->image_pub_ = image_it.advertise(image_topic, queue_size, image_connect_cb,
00087                                          image_disconnect_cb, tracked_object, latch);
00088   impl_->info_pub_ = info_nh.advertise<sensor_msgs::CameraInfo>(info_topic, queue_size, info_connect_cb,
00089                                                                 info_disconnect_cb, tracked_object, latch);
00090 }
00091 
00092 uint32_t CameraPublisher::getNumSubscribers() const
00093 {
00094   if (impl_ && impl_->isValid())
00095     return std::max(impl_->image_pub_.getNumSubscribers(), impl_->info_pub_.getNumSubscribers());
00096   return 0;
00097 }
00098 
00099 std::string CameraPublisher::getTopic() const
00100 {
00101   if (impl_) return impl_->image_pub_.getTopic();
00102   return std::string();
00103 }
00104 
00105 std::string CameraPublisher::getInfoTopic() const
00106 {
00107   if (impl_) return impl_->info_pub_.getTopic();
00108   return std::string();
00109 }
00110 
00111 void CameraPublisher::publish(const sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info) const
00112 {
00113   if (!impl_ || !impl_->isValid()) {
00114     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
00115     return;
00116   }
00117   
00118   impl_->image_pub_.publish(image);
00119   impl_->info_pub_.publish(info);
00120 }
00121 
00122 void CameraPublisher::publish(const sensor_msgs::ImageConstPtr& image,
00123                               const sensor_msgs::CameraInfoConstPtr& info) const
00124 {
00125   if (!impl_ || !impl_->isValid()) {
00126     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
00127     return;
00128   }
00129   
00130   impl_->image_pub_.publish(image);
00131   impl_->info_pub_.publish(info);
00132 }
00133 
00134 void CameraPublisher::publish(sensor_msgs::Image& image, sensor_msgs::CameraInfo& info,
00135                               ros::Time stamp) const
00136 {
00137   if (!impl_ || !impl_->isValid()) {
00138     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
00139     return;
00140   }
00141   
00142   image.header.stamp = stamp;
00143   info.header.stamp = stamp;
00144   publish(image, info);
00145 }
00146 
00147 void CameraPublisher::shutdown()
00148 {
00149   if (impl_) {
00150     impl_->shutdown();
00151     impl_.reset();
00152   }
00153 }
00154 
00155 CameraPublisher::operator void*() const
00156 {
00157   return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00158 }
00159 
00160 } //namespace image_transport


image_transport
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 13:30:17