camera_subscriber.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
40 
41 inline void increment(int* value)
42 {
43  ++(*value);
44 }
45 
46 namespace image_transport {
47 
49 {
50  Impl(uint32_t queue_size)
51  : sync_(queue_size),
52  unsubscribed_(false),
54  {}
55 
57  {
58  shutdown();
59  }
60 
61  bool isValid() const
62  {
63  return !unsubscribed_;
64  }
65 
66  void shutdown()
67  {
68  if (!unsubscribed_) {
69  unsubscribed_ = true;
72  }
73  }
74 
76  {
77  int threshold = 3 * both_received_;
78  if (image_received_ > threshold || info_received_ > threshold) {
79  ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else
80  "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. "
81  "In the last 10s:\n"
82  "\tImage messages received: %d\n"
83  "\tCameraInfo messages received: %d\n"
84  "\tSynchronized pairs: %d",
85  image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(),
87  }
89  }
90 
95  // For detecting when the topics aren't synchronized
98 };
99 
101  const std::string& base_topic, uint32_t queue_size,
102  const Callback& callback, const ros::VoidPtr& tracked_object,
103  const TransportHints& transport_hints)
104  : impl_(new Impl(queue_size))
105 {
106  // Must explicitly remap the image topic since we then do some string manipulation on it
107  // to figure out the sibling camera_info topic.
108  std::string image_topic = info_nh.resolveName(base_topic);
109  std::string info_topic = getCameraInfoTopic(image_topic);
110  impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
111  impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
112  impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
113  // need for Boost.Bind here is kind of broken
114  impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
115 
116  // Complain every 10s if it appears that the image and info topics are not synchronized
117  impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
118  impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
119  impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
120  impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
121  boost::bind(&Impl::checkImagesSynchronized, impl_.get()));
122 }
123 
124 std::string CameraSubscriber::getTopic() const
125 {
126  if (impl_) return impl_->image_sub_.getTopic();
127  return std::string();
128 }
129 
131 {
132  if (impl_) return impl_->info_sub_.getTopic();
133  return std::string();
134 }
135 
137 {
139  //if (impl_) return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers());
140  if (impl_) return impl_->image_sub_.getNumPublishers();
141  return 0;
142 }
143 
145 {
146  if (impl_) return impl_->image_sub_.getTransport();
147  return std::string();
148 }
149 
151 {
152  if (impl_) impl_->shutdown();
153 }
154 
155 CameraSubscriber::operator void*() const
156 {
157  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
158 }
159 
160 } //namespace image_transport
std::string getTopic() const
Image subscription filter.
std::string getInfoTopic() const
Get the camera info topic.
#define ROS_WARN_NAMED(name,...)
std::string resolveName(const std::string &name, bool remap=true) const
std::string getTransport() const
Returns the name of the transport being used.
message_filters::Subscriber< sensor_msgs::CameraInfo > info_sub_
message_filters::TimeSynchronizer< sensor_msgs::Image, sensor_msgs::CameraInfo > sync_
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
IMAGE_TRANSPORT_DECL std::string getCameraInfoTopic(const std::string &base_topic)
Form the camera info topic name, sibling to the base topic.
void shutdown()
Unsubscribe the callback associated with this CameraSubscriber.
boost::function< void(const sensor_msgs::ImageConstPtr &, const sensor_msgs::CameraInfoConstPtr &)> Callback
Stores transport settings for an image topic subscription.
void unsubscribe()
Force immediate unsubscription of this subscriber from its topic.
uint32_t getNumPublishers() const
Returns the number of publishers this subscriber is connected to.
Advertise and subscribe to image topics.
std::string getTopic() const
Get the base topic (on which the raw image is published).
const ros::TransportHints & getRosHints() const
void increment(int *value)


image_transport
Author(s): Patrick Mihelich
autogenerated on Sat Apr 4 2020 03:14:58