compatibility.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, the mcl_3dl authors
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 copyright holder 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 #ifndef COMPATIBILITY_H
31 #define COMPATIBILITY_H
32 
33 #include <ros/ros.h>
34 
35 #include <string>
36 
37 namespace compat
38 {
39 #define STATIC_ASSERT(EXPR) static_assert(EXPR, #EXPR)
40 
41 // Update cycle
42 // 1. Increment current_level and indicate topic changes using mcl_3dl_compat::subscribe/advertise.
43 // Set default_level to supported_level.
44 // 2. Set default_level to current_level.
45 // 3. Increment supported_level and remove old topic names.
46 const int current_level = 1;
47 const int supported_level = 0;
49 
50 STATIC_ASSERT(supported_level <= current_level && current_level <= supported_level + 1);
51 STATIC_ASSERT(supported_level <= default_level && default_level <= current_level);
52 
53 int getCompat()
54 {
55  int compat(default_level);
56  ros::NodeHandle("~").param("compatible", compat, compat);
57 
58  return compat;
59 }
61 {
62  if (getCompat() < supported_level)
63  {
64  const std::string message =
65  "======= [Obsolated] your configuration for " + ros::this_node::getName() + " is outdated =======";
66  ROS_FATAL("%s", message.c_str());
67  ros::shutdown();
68  throw std::runtime_error(message);
69  }
70  else if (getCompat() > current_level)
71  {
72  const std::string message =
73  "======= [Unsupported] your configuration for " + ros::this_node::getName() + " is futuredated =======";
74  ROS_FATAL("%s", message.c_str());
75  ros::shutdown();
76  throw std::runtime_error(message);
77  }
78  else if (getCompat() != current_level)
79  {
81  "======= [Deprecated] %s is run in compatible mode =======\n"
82  "=========================================================\n"
83  "Set _compatible:=%d to switch to new topic namespaces.\n"
84  "Compatible mode will be obsolated in the future update.\n"
85  "=========================================================",
86  ros::this_node::getName().c_str(), current_level);
87  }
88 }
90 {
92  return std::string("~/");
93  if (nh.getUnresolvedNamespace() == std::string())
94  return std::string();
95  return nh.getNamespace() + "/";
96 }
97 template <class M, class T>
99  ros::NodeHandle& nh_new,
100  const std::string& topic_new,
101  ros::NodeHandle& nh_old,
102  const std::string& topic_old,
103  uint32_t queue_size,
104  void (T::*fp)(M) const,
105  T* obj,
106  const ros::TransportHints& transport_hints = ros::TransportHints())
107 {
108  if (getCompat() != current_level)
109  {
110  ROS_ERROR(
111  "Use %s (%s%s) topic instead of %s (%s%s)",
112  nh_new.resolveName(topic_new, false).c_str(),
113  getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(),
114  nh_old.resolveName(topic_old, false).c_str(),
115  getSimplifiedNamespace(nh_old).c_str(), topic_old.c_str());
116  return nh_old.subscribe(topic_old, queue_size, fp, obj, transport_hints);
117  }
118  else
119  {
120  return nh_new.subscribe(topic_new, queue_size, fp, obj, transport_hints);
121  }
122 }
123 template <class M, class T>
125  ros::NodeHandle& nh_new,
126  const std::string& topic_new,
127  ros::NodeHandle& nh_old,
128  const std::string& topic_old,
129  uint32_t queue_size,
130  void (T::*fp)(M),
131  T* obj,
132  const ros::TransportHints& transport_hints = ros::TransportHints())
133 {
134  if (getCompat() != current_level)
135  {
136  ROS_ERROR(
137  "Use %s (%s%s) topic instead of %s (%s%s)",
138  nh_new.resolveName(topic_new, false).c_str(),
139  getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(),
140  nh_old.resolveName(topic_old, false).c_str(),
141  getSimplifiedNamespace(nh_old).c_str(), topic_old.c_str());
142  return nh_old.subscribe(topic_old, queue_size, fp, obj, transport_hints);
143  }
144  else
145  {
146  return nh_new.subscribe(topic_new, queue_size, fp, obj, transport_hints);
147  }
148 }
149 template <class M>
151  ros::NodeHandle& nh_new,
152  const std::string& topic_new,
153  ros::NodeHandle& nh_old,
154  const std::string& topic_old,
155  uint32_t queue_size,
156  const boost::function<void(const boost::shared_ptr<M const>&)>& callback,
157  const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(),
158  const ros::TransportHints& transport_hints = ros::TransportHints())
159 {
160  if (getCompat() != current_level)
161  {
162  ROS_ERROR(
163  "Use %s (%s%s) topic instead of %s (%s%s)",
164  nh_new.resolveName(topic_new, false).c_str(),
165  getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(),
166  nh_old.resolveName(topic_old, false).c_str(),
167  getSimplifiedNamespace(nh_old).c_str(), topic_old.c_str());
168  return nh_old.subscribe(topic_old, queue_size, callback, tracked_object, transport_hints);
169  }
170  else
171  {
172  return nh_new.subscribe(topic_new, queue_size, callback, tracked_object, transport_hints);
173  }
174 }
175 template <class M>
177  ros::NodeHandle& nh_new,
178  const std::string& topic_new,
179  ros::NodeHandle& nh_old,
180  const std::string& topic_old,
181  uint32_t queue_size,
182  bool latch = false)
183 {
184  if (getCompat() != current_level)
185  {
186  ROS_ERROR(
187  "Use %s (%s%s) topic instead of %s (%s%s)",
188  nh_new.resolveName(topic_new, false).c_str(),
189  getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(),
190  nh_old.resolveName(topic_old, false).c_str(),
191  getSimplifiedNamespace(nh_old).c_str(), topic_old.c_str());
192  return nh_old.advertise<M>(topic_old, queue_size, latch);
193  }
194  else
195  {
196  return nh_new.advertise<M>(topic_new, queue_size, latch);
197  }
198 }
199 template <class T, class MReq, class MRes>
201  ros::NodeHandle& nh_new,
202  const std::string& service_new,
203  ros::NodeHandle& nh_old,
204  const std::string& service_old,
205  bool (T::*srv_func)(MReq&, MRes&),
206  T* obj)
207 {
208  if (getCompat() != current_level)
209  {
210  ROS_ERROR(
211  "Use %s (%s%s) service instead of %s (%s%s)",
212  nh_new.resolveName(service_new, false).c_str(),
213  getSimplifiedNamespace(nh_new).c_str(), service_new.c_str(),
214  nh_old.resolveName(service_old, false).c_str(),
215  getSimplifiedNamespace(nh_old).c_str(), service_old.c_str());
216  return nh_old.advertiseService(service_old, srv_func, obj);
217  }
218  else
219  {
220  return nh_new.advertiseService(service_new, srv_func, obj);
221  }
222 }
223 } // namespace compat
224 
225 #endif // COMPATIBILITY_H
const std::string & getUnresolvedNamespace() const
#define ROS_FATAL(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
STATIC_ASSERT(supported_level<=current_level &&current_level<=supported_level+1)
std::string resolveName(const std::string &name, bool remap=true) const
const int supported_level
Definition: compatibility.h:47
ROSCPP_DECL const std::string & getName()
ros::ServiceServer advertiseService(ros::NodeHandle &nh_new, const std::string &service_new, ros::NodeHandle &nh_old, const std::string &service_old, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int getCompat()
Definition: compatibility.h:53
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const int current_level
Definition: compatibility.h:46
#define ROS_ERROR_ONCE(...)
const std::string & getNamespace() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string getSimplifiedNamespace(ros::NodeHandle &nh)
Definition: compatibility.h:89
const int default_level
Definition: compatibility.h:48
void checkCompatMode()
Definition: compatibility.h:60
ros::Publisher advertise(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void shutdown()
ros::Subscriber subscribe(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, void(T::*fp)(M) const, T *obj, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: compatibility.h:98
#define ROS_ERROR(...)


ypspur_ros
Author(s): Atsushi Watanabe
autogenerated on Thu May 13 2021 03:01:26