LaserPublisher.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Chad Rockey
32  */
33 
35 
36 namespace laser_proc {
37 
38 typedef sensor_msgs::LaserScanPtr (*PublishFunction)(const sensor_msgs::MultiEchoLaserScan& msg);
39 
41 {
42  Impl()
43  : unadvertised_(false)
44  {
45  }
46 
48  {
49  shutdown();
50  }
51 
52  bool isValid() const
53  {
54  return !unadvertised_;
55  }
56 
57  void shutdown()
58  {
59  if (!unadvertised_) {
60  unadvertised_ = true;
61  for(size_t i = 0; i < pubs_.size(); i++){
62  pubs_[i].shutdown();
63  }
64  }
65  }
66 
68  std::vector<ros::Publisher> pubs_;
69  std::vector<PublishFunction> functs_;
71 };
72 
75  const ros::SubscriberStatusCallback& connect_cb,
76  const ros::SubscriberStatusCallback& disconnect_cb,
77  const ros::VoidPtr& tracked_object, bool latch, bool publish_echoes)
78  : impl_(new Impl)
79 {
80  if(publish_echoes){
81  impl_->echo_pub_ = nh.advertise<sensor_msgs::MultiEchoLaserScan>("echoes", queue_size, connect_cb, disconnect_cb, tracked_object, latch);
82  }
83  impl_->pubs_.push_back(nh.advertise<sensor_msgs::LaserScan>("first", queue_size, connect_cb, disconnect_cb, tracked_object, latch));
84  impl_->functs_.push_back(laser_proc::LaserProc::getFirstScan);
85  impl_->pubs_.push_back(nh.advertise<sensor_msgs::LaserScan>("last", queue_size, connect_cb, disconnect_cb, tracked_object, latch));
86  impl_->functs_.push_back(laser_proc::LaserProc::getLastScan);
87  impl_->pubs_.push_back(nh.advertise<sensor_msgs::LaserScan>("most_intense", queue_size, connect_cb, disconnect_cb, tracked_object, latch));
89 }
90 
92 {
93  if (impl_ && impl_->isValid()){
94  uint32_t num = impl_->echo_pub_.getNumSubscribers();
95  for(size_t i = 0; i < impl_->pubs_.size(); i++){
96  num = num + impl_->pubs_[i].getNumSubscribers();
97  }
98  return num;
99  }
100  return 0;
101 }
102 
103 std::vector<std::string> LaserPublisher::getTopics() const
104 {
105  std::vector<std::string> topics;
106  topics.push_back(impl_->echo_pub_.getTopic());
107  if (impl_ && impl_->isValid()){
108  for(size_t i = 0; i < impl_->pubs_.size(); i++){
109  topics.push_back(impl_->pubs_[i].getTopic());
110  }
111  }
112  return topics;
113 }
114 
115 void LaserPublisher::publish(const sensor_msgs::MultiEchoLaserScan& msg) const
116 {
117  if (!impl_ || !impl_->isValid()) {
118  ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::LaserPublisher");
119  return;
120  }
121 
122  // Publish original MultiEchoLaserScan
123  if(impl_->echo_pub_){
124  if(impl_->echo_pub_.getNumSubscribers() > 0){
125  impl_->echo_pub_.publish(msg);
126  }
127  }
128 
129  // If needed, publish LaserScans
130  for(size_t i = 0; i < impl_->pubs_.size(); i++){
131  if(impl_->pubs_[i].getNumSubscribers() > 0){
132  try{
133  impl_->pubs_[i].publish(impl_->functs_[i](msg));
134  } catch(std::runtime_error& e){
135  ROS_ERROR_THROTTLE(1.0, "Could not publish to topic %s: %s", impl_->pubs_[i].getTopic().c_str(), e.what());
136  }
137  }
138  }
139 }
140 
141 void LaserPublisher::publish(const sensor_msgs::MultiEchoLaserScanConstPtr& msg) const
142 {
143  if (!impl_ || !impl_->isValid()) {
144  ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::LaserPublisher");
145  return;
146  }
147 
148  // Publish original MultiEchoLaserScan
149  if(impl_->echo_pub_){
150  impl_->echo_pub_.publish(msg);
151  }
152 
153  // If needed, publish LaserScans
154  for(size_t i = 0; i < impl_->pubs_.size(); i++){
155  if(impl_->pubs_[i].getNumSubscribers() > 0){
156  try{
157  impl_->pubs_[i].publish(impl_->functs_[i](*msg));
158  } catch(std::runtime_error& e){
159  ROS_ERROR_THROTTLE(1.0, "Could not publish to topic %s: %s", impl_->pubs_[i].getTopic().c_str(), e.what());
160  }
161  }
162  }
163 }
164 
166 {
167  if (impl_) {
168  impl_->shutdown();
169  impl_.reset();
170  }
171 }
172 
173 LaserPublisher::operator void*() const
174 {
175  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
176 }
177 
178 } //namespace laser_proc
179 
static sensor_msgs::LaserScanPtr getFirstScan(const sensor_msgs::MultiEchoLaserScan &msg)
Definition: LaserProc.cpp:38
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
uint32_t getNumSubscribers() const
Returns the number of subscribers that are currently connected to this LaserPublisher.
static sensor_msgs::LaserScanPtr getMostIntenseScan(const sensor_msgs::MultiEchoLaserScan &msg)
Definition: LaserProc.cpp:79
std::vector< std::string > getTopics() const
Returns the topics of this LaserPublisher.
Make a static class that creates these
#define ROS_ERROR_THROTTLE(rate,...)
std::vector< ros::Publisher > pubs_
#define ROS_ASSERT_MSG(cond,...)
sensor_msgs::LaserScanPtr(* PublishFunction)(const sensor_msgs::MultiEchoLaserScan &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static sensor_msgs::LaserScanPtr getLastScan(const sensor_msgs::MultiEchoLaserScan &msg)
Definition: LaserProc.cpp:59
void shutdown()
Shutdown the advertisements associated with this Publisher.
void publish(const sensor_msgs::MultiEchoLaserScan &msg) const
Publish a MultiEchoLaserScan on the topics associated with this LaserPublisher.
std::vector< PublishFunction > functs_


laser_proc
Author(s): Chad Rockey
autogenerated on Mon Jun 10 2019 13:45:29