nodelet.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2016, Ryohei Ueda.
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 Kei Okada 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 
35 #ifndef OPENCV_APPS_NODELET_H_
36 #define OPENCV_APPS_NODELET_H_
37 
38 #include <ros/ros.h>
39 #include <nodelet/nodelet.h>
40 #include <boost/thread.hpp>
42 
43 // https://stackoverflow.com/questions/10496824/how-to-define-nullptr-for-supporting-both-c03-and-c11
44 #if !defined(nullptr)
45 #define nullptr NULL
46 #endif
47 
48 namespace opencv_apps
49 {
54 {
58 };
59 
70 class Nodelet : public nodelet::Nodelet
71 {
72 public:
73  Nodelet() : subscribed_(false)
74  {
75  }
76 
77 protected:
82  virtual void onInit();
83 
89  virtual void onInitPostProcess();
90 
94  virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub);
95 
101 
107 
113 
120  virtual void cameraConnectionBaseCallback();
121 
126  virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent& event);
127 
133  virtual void subscribe() = 0;
134 
140  virtual void unsubscribe() = 0;
141 
154  template <class T>
155  ros::Publisher advertise(ros::NodeHandle& nh, std::string topic, int queue_size)
156  {
157  boost::mutex::scoped_lock lock(connection_mutex_);
158  ros::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::connectionCallback, this, _1);
159  ros::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::connectionCallback, this, _1);
160  bool latch;
161  nh.param("latch", latch, false);
162  ros::Publisher ret = nh.advertise<T>(topic, queue_size, connect_cb, disconnect_cb, ros::VoidConstPtr(), latch);
163  publishers_.push_back(ret);
164 
165  return ret;
166  }
167 
180  image_transport::Publisher advertiseImage(ros::NodeHandle& nh, const std::string& topic, int queue_size)
181  {
182  boost::mutex::scoped_lock lock(connection_mutex_);
184  image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::imageConnectionCallback, this, _1);
185  bool latch;
186  nh.param("latch", latch, false);
188  image_transport::ImageTransport(nh).advertise(topic, 1, connect_cb, disconnect_cb, ros::VoidPtr(), latch);
189  image_publishers_.push_back(pub);
190  return pub;
191  }
192 
206  image_transport::CameraPublisher advertiseCamera(ros::NodeHandle& nh, const std::string& topic, int queue_size)
207  {
208  boost::mutex::scoped_lock lock(connection_mutex_);
211  ros::SubscriberStatusCallback info_connect_cb = boost::bind(&Nodelet::cameraInfoConnectionCallback, this, _1);
212  ros::SubscriberStatusCallback info_disconnect_cb = boost::bind(&Nodelet::cameraInfoConnectionCallback, this, _1);
213  bool latch;
214  nh.param("latch", latch, false);
216  topic, 1, connect_cb, disconnect_cb, info_connect_cb, info_disconnect_cb, ros::VoidPtr(), latch);
217  camera_publishers_.push_back(pub);
218  return pub;
219  }
220 
225  boost::mutex connection_mutex_;
226 
230  std::vector<ros::Publisher> publishers_;
231 
235  std::vector<image_transport::Publisher> image_publishers_;
236 
240  std::vector<image_transport::CameraPublisher> camera_publishers_;
241 
246 
251 
256 
262 
268 
274 
279 
284 
285 private:
286 };
287 }
288 
289 #endif
boost::shared_ptr< void const > VoidConstPtr
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
bool verbose_connection_
true if ~verbose_connection or verbose_connection parameter is true.
Definition: nodelet.h:283
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
virtual void imageConnectionCallback(const image_transport::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come for image publisher
Definition: nodelet.cpp:114
Demo code to calculate moments.
Definition: nodelet.h:48
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
virtual void unsubscribe()=0
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
virtual void cameraConnectionBaseCallback()
callback function which is called when new subscriber come for camera image publisher or camera info ...
Definition: nodelet.cpp:165
virtual void subscribe()=0
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
ros::WallTimer timer_
WallTimer instance for warning about no connection.
Definition: nodelet.h:255
std::vector< image_transport::Publisher > image_publishers_
List of watching image publishers.
Definition: nodelet.h:235
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
Definition: nodelet.cpp:57
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Definition: nodelet.cpp:40
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void cameraInfoConnectionCallback(const ros::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come for camera info publisher ...
Definition: nodelet.cpp:160
virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent &event)
callback function which is called when walltimer duration run out.
Definition: nodelet.cpp:65
ConnectionStatus
Enum to represent connection status.
Definition: nodelet.h:53
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
Definition: nodelet.h:180
image_transport::CameraPublisher advertiseCamera(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic camera info topic and watch the publisher. Publishers which are created by t...
Definition: nodelet.h:206
ConnectionStatus connection_status_
Status of connection.
Definition: nodelet.h:278
bool ever_subscribed_
A flag to check if the node has been ever subscribed or not.
Definition: nodelet.h:267
virtual void cameraConnectionCallback(const image_transport::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come for camera image publisher ...
Definition: nodelet.cpp:155
bool subscribed_
A flag to check if any publisher is already subscribed or not.
Definition: nodelet.h:261
ros::Publisher advertise(ros::NodeHandle &nh, std::string topic, int queue_size)
Advertise a topic and watch the publisher. Publishers which are created by this method. It automatically reads latch boolean parameter from nh and publish topic with appropriate latch parameter.
Definition: nodelet.h:155
boost::mutex connection_mutex_
mutex to call subscribe() and unsubscribe() in critical section.
Definition: nodelet.h:225
std::vector< image_transport::CameraPublisher > camera_publishers_
List of watching camera publishers.
Definition: nodelet.h:240
virtual void connectionCallback(const ros::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come
Definition: nodelet.cpp:73
std::vector< ros::Publisher > publishers_
List of watching publishers.
Definition: nodelet.h:230


opencv_apps
Author(s): Kei Okada
autogenerated on Sat Aug 22 2020 03:35:08