publication_server.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 
37 
38 namespace polled_camera {
39 
41 
42 class PublicationServer::Impl
43 {
44 public:
45  ros::ServiceServer srv_server_;
46  DriverCallback driver_cb_;
47  ros::VoidPtr tracked_object_;
49  std::map<std::string, image_transport::CameraPublisher> client_map_;
50  bool unadvertised_;
51  double constructed_;
52 
53  Impl(const ros::NodeHandle& nh)
54  : it_(nh),
55  unadvertised_(false),
56  constructed_(ros::WallTime::now().toSec())
57  {
58  }
59 
60  ~Impl()
61  {
62  if (ros::WallTime::now().toSec() - constructed_ < 0.001)
63  ROS_WARN("PublicationServer destroyed immediately after creation. Did you forget to store the handle?");
64  unadvertise();
65  }
66 
67  bool isValid() const
68  {
69  return !unadvertised_;
70  }
71 
72  void unadvertise()
73  {
74  if (!unadvertised_) {
75  unadvertised_ = true;
76  srv_server_.shutdown();
77  client_map_.clear();
78  }
79  }
80 
81  bool requestCallback(polled_camera::GetPolledImage::Request& req,
82  polled_camera::GetPolledImage::Response& rsp)
83  {
84  std::string image_topic = req.response_namespace + "/image_raw";
85  image_transport::CameraPublisher& pub = client_map_[image_topic];
86  if (!pub) {
87  // Create a latching camera publisher.
89  boost::bind(&Impl::disconnectCallback, this, boost::placeholders::_1),
91  ros::VoidPtr(), true /*latch*/);
92  ROS_INFO("Advertising %s", pub.getTopic().c_str());
93  }
94 
95  // Correct zero binning values to one for convenience
96  req.binning_x = std::max(req.binning_x, (uint32_t)1);
97  req.binning_y = std::max(req.binning_y, (uint32_t)1);
98 
100  sensor_msgs::Image image;
101  sensor_msgs::CameraInfo info;
102  driver_cb_(req, rsp, image, info);
103 
104  if (rsp.success) {
105  assert(image.header.stamp == info.header.stamp);
106  rsp.stamp = image.header.stamp;
107  pub.publish(image, info);
108  }
109  else {
110  ROS_ERROR("Failed to capture requested image, status message: '%s'",
111  rsp.status_message.c_str());
112  }
113 
114  return true; // Success/failure indicated by rsp.success
115  }
116 
117  void disconnectCallback(const image_transport::SingleSubscriberPublisher& ssp)
118  {
119  // Shut down the publication when the subscription count drops to zero.
120  if (ssp.getNumSubscribers() == 0) {
121  ROS_INFO("Shutting down %s", ssp.getTopic().c_str());
122  client_map_.erase(ssp.getTopic());
123  }
124  }
125 };
126 
128 
129 PublicationServer::PublicationServer(const std::string& service, ros::NodeHandle& nh,
130  const DriverCallback& cb, const ros::VoidPtr& tracked_object)
131  : impl_(new Impl(nh))
132 {
133  impl_->driver_cb_ = cb;
134  impl_->tracked_object_ = tracked_object;
135  impl_->srv_server_ = nh.advertiseService<>(service, &Impl::requestCallback, impl_);
136 }
137 
138 void PublicationServer::shutdown()
139 {
140  if (impl_) impl_->unadvertise();
141 }
142 
143 std::string PublicationServer::getService() const
144 {
145  if (impl_) return impl_->srv_server_.getService();
146  return std::string();
147 }
148 
149 PublicationServer::operator void*() const
150 {
151  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
152 }
153 
154 PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
155  const PublicationServer::DriverCallback& cb,
156  const ros::VoidPtr& tracked_object)
157 {
158  return PublicationServer(service, nh, cb, tracked_object);
159 }
160 
161 } //namespace polled_camera
image_transport::ImageTransport
boost::shared_ptr< void >
ros
polled_camera
Definition: publication_server.h:57
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
publication_server.h
ros::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
ros::ServiceServer
image_transport::SingleSubscriberPublisher::getNumSubscribers
uint32_t getNumSubscribers() const
polled_camera::PublicationServer::PublicationServer
PublicationServer()
Definition: publication_server.h:88
ros::WallTime::now
static WallTime now()
image_transport::CameraPublisher
ROS_WARN
#define ROS_WARN(...)
image_transport::CameraPublisher::getTopic
std::string getTopic() const
polled_camera::advertise
PublicationServer advertise(ros::NodeHandle &nh, const std::string &service, const PublicationServer::DriverCallback &cb, const ros::VoidPtr &tracked_object=ros::VoidPtr())
Advertise a polled image service, version for arbitrary boost::function object.
Definition: publication_server.cpp:186
image_transport::SingleSubscriberPublisher
image_transport.h
image_transport::SingleSubscriberPublisher::getTopic
std::string getTopic() const
ROS_ERROR
#define ROS_ERROR(...)
ros::ServiceServer::getService
std::string getService() const
image_transport::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::ServiceServer::shutdown
void shutdown()
ROS_INFO
#define ROS_INFO(...)
polled_camera::PublicationServer::DriverCallback
boost::function< void(polled_camera::GetPolledImage::Request &, polled_camera::GetPolledImage::Response &, sensor_msgs::Image &, sensor_msgs::CameraInfo &)> DriverCallback
Definition: publication_server.h:86
ros::NodeHandle


polled_camera
Author(s): Patrick Mihelich
autogenerated on Sat Jan 20 2024 03:14:56