publication_server.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 "polled_camera/publication_server.h"
00036 #include <image_transport/image_transport.h>
00037 
00038 namespace polled_camera {
00039 
00041 
00042 class PublicationServer::Impl
00043 {
00044 public:
00045   ros::ServiceServer srv_server_;
00046   DriverCallback driver_cb_;
00047   ros::VoidPtr tracked_object_;
00048   image_transport::ImageTransport it_;
00049   std::map<std::string, image_transport::CameraPublisher> client_map_;
00050   bool unadvertised_;
00051   double constructed_;
00052   
00053   Impl(const ros::NodeHandle& nh)
00054     : it_(nh),
00055       unadvertised_(false),
00056       constructed_(ros::WallTime::now().toSec())
00057   {
00058   }
00059   
00060   ~Impl()
00061   {
00062     if (ros::WallTime::now().toSec() - constructed_ < 0.001)
00063       ROS_WARN("PublicationServer destroyed immediately after creation. Did you forget to store the handle?");
00064     unadvertise();
00065   }
00066 
00067   bool isValid() const
00068   {
00069     return !unadvertised_;
00070   }
00071 
00072   void unadvertise()
00073   {
00074     if (!unadvertised_) {
00075       unadvertised_ = true;
00076       srv_server_.shutdown();
00077       client_map_.clear();
00078     }
00079   }
00080 
00081   bool requestCallback(polled_camera::GetPolledImage::Request& req,
00082                        polled_camera::GetPolledImage::Response& rsp)
00083   {
00084     std::string image_topic = req.response_namespace + "/image_raw";
00085     image_transport::CameraPublisher& pub = client_map_[image_topic];
00086     if (!pub) {
00087       // Create a latching camera publisher.
00088       pub = it_.advertiseCamera(image_topic, 1, image_transport::SubscriberStatusCallback(),
00089                                 boost::bind(&Impl::disconnectCallback, this, _1),
00090                                 ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
00091                                 ros::VoidPtr(), true /*latch*/);
00092       ROS_INFO("Advertising %s", pub.getTopic().c_str());
00093     }
00094 
00095     // Correct zero binning values to one for convenience
00096     req.binning_x = std::max(req.binning_x, (uint32_t)1);
00097     req.binning_y = std::max(req.binning_y, (uint32_t)1);
00098 
00100     sensor_msgs::Image image;
00101     sensor_msgs::CameraInfo info;
00102     driver_cb_(req, rsp, image, info);
00103     
00104     if (rsp.success) {
00105       assert(image.header.stamp == info.header.stamp);
00106       rsp.stamp = image.header.stamp;
00107       pub.publish(image, info);
00108     }
00109     else {
00110       ROS_ERROR("Failed to capture requested image, status message: '%s'",
00111                 rsp.status_message.c_str());
00112     }
00113     
00114     return true; // Success/failure indicated by rsp.success
00115   }
00116 
00117   void disconnectCallback(const image_transport::SingleSubscriberPublisher& ssp)
00118   {
00119     // Shut down the publication when the subscription count drops to zero.
00120     if (ssp.getNumSubscribers() == 0) {
00121       ROS_INFO("Shutting down %s", ssp.getTopic().c_str());
00122       client_map_.erase(ssp.getTopic());
00123     }
00124   }
00125 };
00126 
00128 
00129 PublicationServer::PublicationServer(const std::string& service, ros::NodeHandle& nh,
00130                                      const DriverCallback& cb, const ros::VoidPtr& tracked_object)
00131   : impl_(new Impl(nh))
00132 {
00133   impl_->driver_cb_ = cb;
00134   impl_->tracked_object_ = tracked_object;
00135   impl_->srv_server_ = nh.advertiseService<>(service, &Impl::requestCallback, impl_);
00136 }
00137 
00138 void PublicationServer::shutdown()
00139 {
00140   if (impl_) impl_->unadvertise();
00141 }
00142 
00143 std::string PublicationServer::getService() const
00144 {
00145   if (impl_) return impl_->srv_server_.getService();
00146   return std::string();
00147 }
00148 
00149 PublicationServer::operator void*() const
00150 {
00151   return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00152 }
00153 
00154 PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
00155                             const PublicationServer::DriverCallback& cb,
00156                             const ros::VoidPtr& tracked_object)
00157 {
00158   return PublicationServer(service, nh, cb, tracked_object);
00159 }
00160 
00161 } //namespace polled_camera


polled_camera
Author(s): Patrick Mihelich
autogenerated on Thu Jun 6 2019 21:20:04