camera_subscriber.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/camera_subscriber.h"
00036 #include "image_transport/subscriber_filter.h"
00037 #include "image_transport/camera_common.h"
00038 #include <message_filters/subscriber.h>
00039 #include <message_filters/time_synchronizer.h>
00040 
00041 inline void increment(int* value)
00042 {
00043   ++(*value);
00044 }
00045 
00046 namespace image_transport {
00047 
00048 struct CameraSubscriber::Impl
00049 {
00050   Impl(uint32_t queue_size)
00051     : sync_(queue_size),
00052       unsubscribed_(false),
00053       image_received_(0), info_received_(0), both_received_(0)
00054   {}
00055 
00056   ~Impl()
00057   {
00058     shutdown();
00059   }
00060 
00061   bool isValid() const
00062   {
00063     return !unsubscribed_;
00064   }
00065   
00066   void shutdown()
00067   {
00068     if (!unsubscribed_) {
00069       unsubscribed_ = true;
00070       image_sub_.unsubscribe();
00071       info_sub_.unsubscribe();
00072     }
00073   }
00074 
00075   void checkImagesSynchronized()
00076   {
00077     int threshold = 3 * both_received_;
00078     if (image_received_ > threshold || info_received_ > threshold) {
00079       ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else
00080                      "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. "
00081                      "In the last 10s:\n"
00082                      "\tImage messages received:      %d\n"
00083                      "\tCameraInfo messages received: %d\n"
00084                      "\tSynchronized pairs:           %d",
00085                      image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(),
00086                      image_received_, info_received_, both_received_);
00087     }
00088     image_received_ = info_received_ = both_received_ = 0;
00089   }
00090   
00091   SubscriberFilter image_sub_;
00092   message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00093   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
00094   bool unsubscribed_;
00095   // For detecting when the topics aren't synchronized
00096   ros::WallTimer check_synced_timer_;
00097   int image_received_, info_received_, both_received_;
00098 };
00099 
00100 CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
00101                                    const std::string& base_topic, uint32_t queue_size,
00102                                    const Callback& callback, const ros::VoidPtr& tracked_object,
00103                                    const TransportHints& transport_hints)
00104   : impl_(new Impl(queue_size))
00105 {
00106   // Must explicitly remap the image topic since we then do some string manipulation on it
00107   // to figure out the sibling camera_info topic.
00108   std::string image_topic = info_nh.resolveName(base_topic);
00109   std::string info_topic = getCameraInfoTopic(image_topic);
00110   impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
00111   impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
00112   impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
00113   // need for Boost.Bind here is kind of broken
00114   impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
00115 
00116   // Complain every 10s if it appears that the image and info topics are not synchronized
00117   impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
00118   impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
00119   impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
00120   impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
00121                                                        boost::bind(&Impl::checkImagesSynchronized, impl_.get()));
00122 }
00123 
00124 std::string CameraSubscriber::getTopic() const
00125 {
00126   if (impl_) return impl_->image_sub_.getTopic();
00127   return std::string();
00128 }
00129 
00130 std::string CameraSubscriber::getInfoTopic() const
00131 {
00132   if (impl_) return impl_->info_sub_.getTopic();
00133   return std::string();
00134 }
00135 
00136 uint32_t CameraSubscriber::getNumPublishers() const
00137 {
00139   //if (impl_) return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers());
00140   if (impl_) return impl_->image_sub_.getNumPublishers();
00141   return 0;
00142 }
00143 
00144 std::string CameraSubscriber::getTransport() const
00145 {
00146   if (impl_) return impl_->image_sub_.getTransport();
00147   return std::string();
00148 }
00149 
00150 void CameraSubscriber::shutdown()
00151 {
00152   if (impl_) impl_->shutdown();
00153 }
00154 
00155 CameraSubscriber::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