nodelet.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, 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 #ifndef NODELET_NODELET_H
31 #define NODELET_NODELET_H
32 
33 #include "nodeletdecl.h"
34 #include "exception.h"
35 
36 #include <string>
37 #include <vector>
38 #include <map>
39 
40 #include <ros/console.h>
41 #include <boost/shared_ptr.hpp>
42 
43 namespace ros
44 {
45 class NodeHandle;
46 class CallbackQueueInterface;
47 }
48 
49 #define NODELET_DEBUG(...) ROS_DEBUG_NAMED(getName(), __VA_ARGS__)
50 #define NODELET_DEBUG_STREAM(...) ROS_DEBUG_STREAM_NAMED(getName(), __VA_ARGS__)
51 #define NODELET_DEBUG_ONCE(...) ROS_DEBUG_ONCE_NAMED(getName(), __VA_ARGS__)
52 #define NODELET_DEBUG_STREAM_ONCE(...) ROS_DEBUG_STREAM_ONCE_NAMED(getName(), __VA_ARGS__)
53 #define NODELET_DEBUG_COND(cond, ...) ROS_DEBUG_COND_NAMED(cond, getName(), __VA_ARGS__)
54 #define NODELET_DEBUG_STREAM_COND(cond, ...) ROS_DEBUG_STREAM_COND_NAMED(cond, getName(), __VA_ARGS__)
55 #define NODELET_DEBUG_THROTTLE(rate, ...) ROS_DEBUG_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
56 #define NODELET_DEBUG_STREAM_THROTTLE(rate, ...) ROS_DEBUG_STREAM_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
57 #define NODELET_DEBUG_FILTER(filter, ...) ROS_DEBUG_FILTER_NAMED(filter, getName(), __VA_ARGS__)
58 #define NODELET_DEBUG_STREAM_FILTER(filter, ...) ROS_DEBUG_STREAM_FILTER_NAMED(filter, getName(), __VA_ARGS__)
59 
60 #define NODELET_INFO(...) ROS_INFO_NAMED(getName(), __VA_ARGS__)
61 #define NODELET_INFO_STREAM(...) ROS_INFO_STREAM_NAMED(getName(), __VA_ARGS__)
62 #define NODELET_INFO_ONCE(...) ROS_INFO_ONCE_NAMED(getName(), __VA_ARGS__)
63 #define NODELET_INFO_STREAM_ONCE(...) ROS_INFO_STREAM_ONCE_NAMED(getName(), __VA_ARGS__)
64 #define NODELET_INFO_COND(cond, ...) ROS_INFO_COND_NAMED(cond, getName(), __VA_ARGS__)
65 #define NODELET_INFO_STREAM_COND(cond, ...) ROS_INFO_STREAM_COND_NAMED(cond, getName(), __VA_ARGS__)
66 #define NODELET_INFO_THROTTLE(rate, ...) ROS_INFO_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
67 #define NODELET_INFO_STREAM_THROTTLE(rate, ...) ROS_INFO_STREAM_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
68 #define NODELET_INFO_FILTER(filter, ...) ROS_INFO_FILTER_NAMED(filter, getName(), __VA_ARGS__)
69 #define NODELET_INFO_STREAM_FILTER(filter, ...) ROS_INFO_STREAM_FILTER_NAMED(filter, getName(), __VA_ARGS__)
70 
71 #define NODELET_WARN(...) ROS_WARN_NAMED(getName(), __VA_ARGS__)
72 #define NODELET_WARN_STREAM(...) ROS_WARN_STREAM_NAMED(getName(), __VA_ARGS__)
73 #define NODELET_WARN_ONCE(...) ROS_WARN_ONCE_NAMED(getName(), __VA_ARGS__)
74 #define NODELET_WARN_STREAM_ONCE(...) ROS_WARN_STREAM_ONCE_NAMED(getName(), __VA_ARGS__)
75 #define NODELET_WARN_COND(cond, ...) ROS_WARN_COND_NAMED(cond, getName(), __VA_ARGS__)
76 #define NODELET_WARN_STREAM_COND(cond, ...) ROS_WARN_STREAM_COND_NAMED(cond, getName(), __VA_ARGS__)
77 #define NODELET_WARN_THROTTLE(rate, ...) ROS_WARN_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
78 #define NODELET_WARN_STREAM_THROTTLE(rate, ...) ROS_WARN_STREAM_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
79 #define NODELET_WARN_FILTER(filter, ...) ROS_WARN_FILTER_NAMED(filter, getName(), __VA_ARGS__)
80 #define NODELET_WARN_STREAM_FILTER(filter, ...) ROS_WARN_STREAM_FILTER_NAMED(filter, getName(), __VA_ARGS__)
81 
82 #define NODELET_ERROR(...) ROS_ERROR_NAMED(getName(), __VA_ARGS__)
83 #define NODELET_ERROR_STREAM(...) ROS_ERROR_STREAM_NAMED(getName(), __VA_ARGS__)
84 #define NODELET_ERROR_ONCE(...) ROS_ERROR_ONCE_NAMED(getName(), __VA_ARGS__)
85 #define NODELET_ERROR_STREAM_ONCE(...) ROS_ERROR_STREAM_ONCE_NAMED(getName(), __VA_ARGS__)
86 #define NODELET_ERROR_COND(cond, ...) ROS_ERROR_COND_NAMED(cond, getName(), __VA_ARGS__)
87 #define NODELET_ERROR_STREAM_COND(cond, ...) ROS_ERROR_STREAM_COND_NAMED(cond, getName(), __VA_ARGS__)
88 #define NODELET_ERROR_THROTTLE(rate, ...) ROS_ERROR_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
89 #define NODELET_ERROR_STREAM_THROTTLE(rate, ...) ROS_ERROR_STREAM_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
90 #define NODELET_ERROR_FILTER(filter, ...) ROS_ERROR_FILTER_NAMED(filter, getName(), __VA_ARGS__)
91 #define NODELET_ERROR_STREAM_FILTER(filter, ...) ROS_ERROR_STREAM_FILTER_NAMED(filter, getName(), __VA_ARGS__)
92 
93 #define NODELET_FATAL(...) ROS_FATAL_NAMED(getName(), __VA_ARGS__)
94 #define NODELET_FATAL_STREAM(...) ROS_FATAL_STREAM_NAMED(getName(), __VA_ARGS__)
95 #define NODELET_FATAL_ONCE(...) ROS_FATAL_ONCE_NAMED(getName(), __VA_ARGS__)
96 #define NODELET_FATAL_STREAM_ONCE(...) ROS_FATAL_STREAM_ONCE_NAMED(getName(), __VA_ARGS__)
97 #define NODELET_FATAL_COND(cond, ...) ROS_FATAL_COND_NAMED(cond, getName(), __VA_ARGS__)
98 #define NODELET_FATAL_STREAM_COND(cond, ...) ROS_FATAL_STREAM_COND_NAMED(cond, getName(), __VA_ARGS__)
99 #define NODELET_FATAL_THROTTLE(rate, ...) ROS_FATAL_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
100 #define NODELET_FATAL_STREAM_THROTTLE(rate, ...) ROS_FATAL_STREAM_THROTTLE_NAMED(rate, getName(), __VA_ARGS__)
101 #define NODELET_FATAL_FILTER(filter, ...) ROS_FATAL_FILTER_NAMED(filter, getName(), __VA_ARGS__)
102 #define NODELET_FATAL_STREAM_FILTER(filter, ...) ROS_FATAL_STREAM_FILTER_NAMED(filter, getName(), __VA_ARGS__)
103 
104 // named versions of the macros
105 #define NODELET_DEBUG_NAMED(suffix, ...) ROS_DEBUG_NAMED(getSuffixedName(suffix), __VA_ARGS__)
106 #define NODELET_DEBUG_STREAM_NAMED(suffix, ...) ROS_DEBUG_STREAM_NAMED(getSuffixedName(suffix), __VA_ARGS__)
107 #define NODELET_DEBUG_ONCE_NAMED(suffix, ...) ROS_DEBUG_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
108 #define NODELET_DEBUG_STREAM_ONCE_NAMED(suffix, ...) ROS_DEBUG_STREAM_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
109 #define NODELET_DEBUG_COND_NAMED(cond, suffix, ...) ROS_DEBUG_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
110 #define NODELET_DEBUG_STREAM_COND_NAMED(cond, suffix, ...) ROS_DEBUG_STREAM_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
111 #define NODELET_DEBUG_THROTTLE_NAMED(rate, suffix, ...) ROS_DEBUG_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
112 #define NODELET_DEBUG_STREAM_THROTTLE_NAMED(rate, suffix, ...) ROS_DEBUG_STREAM_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
113 #define NODELET_DEBUG_FILTER_NAMED(filter, suffix, ...) ROS_DEBUG_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
114 #define NODELET_DEBUG_STREAM_FILTER_NAMED(filter, suffix, ...) ROS_DEBUG_STREAM_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
115 
116 #define NODELET_INFO_NAMED(suffix, ...) ROS_INFO_NAMED(getSuffixedName(suffix), __VA_ARGS__)
117 #define NODELET_INFO_STREAM_NAMED(suffix, ...) ROS_INFO_STREAM_NAMED(getSuffixedName(suffix), __VA_ARGS__)
118 #define NODELET_INFO_ONCE_NAMED(suffix, ...) ROS_INFO_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
119 #define NODELET_INFO_STREAM_ONCE_NAMED(suffix, ...) ROS_INFO_STREAM_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
120 #define NODELET_INFO_COND_NAMED(cond, suffix, ...) ROS_INFO_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
121 #define NODELET_INFO_STREAM_COND_NAMED(cond, suffix, ...) ROS_INFO_STREAM_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
122 #define NODELET_INFO_THROTTLE_NAMED(rate, suffix, ...) ROS_INFO_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
123 #define NODELET_INFO_STREAM_THROTTLE_NAMED(rate, suffix, ...) ROS_INFO_STREAM_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
124 #define NODELET_INFO_FILTER_NAMED(filter, suffix, ...) ROS_INFO_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
125 #define NODELET_INFO_STREAM_FILTER_NAMED(filter, suffix, ...) ROS_INFO_STREAM_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
126 
127 #define NODELET_WARN_NAMED(suffix, ...) ROS_WARN_NAMED(getSuffixedName(suffix), __VA_ARGS__)
128 #define NODELET_WARN_STREAM_NAMED(suffix, ...) ROS_WARN_STREAM_NAMED(getSuffixedName(suffix), __VA_ARGS__)
129 #define NODELET_WARN_ONCE_NAMED(suffix, ...) ROS_WARN_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
130 #define NODELET_WARN_STREAM_ONCE_NAMED(suffix, ...) ROS_WARN_STREAM_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
131 #define NODELET_WARN_COND_NAMED(cond, suffix, ...) ROS_WARN_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
132 #define NODELET_WARN_STREAM_COND_NAMED(cond, suffix, ...) ROS_WARN_STREAM_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
133 #define NODELET_WARN_THROTTLE_NAMED(rate, suffix, ...) ROS_WARN_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
134 #define NODELET_WARN_STREAM_THROTTLE_NAMED(rate, suffix, ...) ROS_WARN_STREAM_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
135 #define NODELET_WARN_FILTER_NAMED(filter, suffix, ...) ROS_WARN_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
136 #define NODELET_WARN_STREAM_FILTER_NAMED(filter, suffix, ...) ROS_WARN_STREAM_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
137 
138 #define NODELET_ERROR_NAMED(suffix, ...) ROS_ERROR_NAMED(getSuffixedName(suffix), __VA_ARGS__)
139 #define NODELET_ERROR_STREAM_NAMED(suffix, ...) ROS_ERROR_STREAM_NAMED(getSuffixedName(suffix), __VA_ARGS__)
140 #define NODELET_ERROR_ONCE_NAMED(suffix, ...) ROS_ERROR_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
141 #define NODELET_ERROR_STREAM_ONCE_NAMED(suffix, ...) ROS_ERROR_STREAM_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
142 #define NODELET_ERROR_COND_NAMED(cond, suffix, ...) ROS_ERROR_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
143 #define NODELET_ERROR_STREAM_COND_NAMED(cond, suffix, ...) ROS_ERROR_STREAM_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
144 #define NODELET_ERROR_THROTTLE_NAMED(rate, suffix, ...) ROS_ERROR_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
145 #define NODELET_ERROR_STREAM_THROTTLE_NAMED(rate, suffix, ...) ROS_ERROR_STREAM_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
146 #define NODELET_ERROR_FILTER_NAMED(filter, suffix, ...) ROS_ERROR_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
147 #define NODELET_ERROR_STREAM_FILTER_NAMED(filter, suffix, ...) ROS_ERROR_STREAM_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
148 
149 #define NODELET_FATAL_NAMED(suffix, ...) ROS_FATAL_NAMED(getSuffixedName(suffix), __VA_ARGS__)
150 #define NODELET_FATAL_STREAM_NAMED(suffix, ...) ROS_FATAL_STREAM_NAMED(getSuffixedName(suffix), __VA_ARGS__)
151 #define NODELET_FATAL_ONCE_NAMED(suffix, ...) ROS_FATAL_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
152 #define NODELET_FATAL_STREAM_ONCE_NAMED(suffix, ...) ROS_FATAL_STREAM_ONCE_NAMED(getSuffixedName(suffix), __VA_ARGS__)
153 #define NODELET_FATAL_COND_NAMED(cond, suffix, ...) ROS_FATAL_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
154 #define NODELET_FATAL_STREAM_COND_NAMED(cond, suffix, ...) ROS_FATAL_STREAM_COND_NAMED(cond, getSuffixedName(suffix), __VA_ARGS__)
155 #define NODELET_FATAL_THROTTLE_NAMED(rate, suffix, ...) ROS_FATAL_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
156 #define NODELET_FATAL_STREAM_THROTTLE_NAMED(rate, suffix, ...) ROS_FATAL_STREAM_THROTTLE_NAMED(rate, getSuffixedName(suffix), __VA_ARGS__)
157 #define NODELET_FATAL_FILTER_NAMED(filter, suffix, ...) ROS_FATAL_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
158 #define NODELET_FATAL_STREAM_FILTER_NAMED(filter, suffix, ...) ROS_FATAL_STREAM_FILTER_NAMED(filter, getSuffixedName(suffix), __VA_ARGS__)
159 
160 namespace nodelet
161 {
163 typedef std::map<std::string, std::string> M_string;
164 typedef std::vector<std::string> V_string;
165 
166 namespace detail {
167  class CallbackQueue;
168 }
169 
171 {
172 public:
173  UninitializedException(const std::string& method_name)
174  : Exception("Calling [" + method_name + "] before the Nodelet is initialized is not allowed.")
175  {}
176 };
177 
179 {
180 public:
182  : Exception("Initialized multiple times")
183  {}
184 };
185 
187 {
188  // Protected data fields for use by the subclass.
189 protected:
190  inline const std::string& getName() const { return nodelet_name_; }
191  inline std::string getSuffixedName(const std::string& suffix) const
192  {
193  return nodelet_name_ + "." + suffix;
194  }
195  inline const V_string& getMyArgv() const { return my_argv_; }
196  inline const M_string& getRemappingArgs() const { return remapping_args_; }
197 
198  ros::NodeHandle& getNodeHandle() const;
199  ros::NodeHandle& getPrivateNodeHandle() const;
200  ros::NodeHandle& getMTNodeHandle() const;
201  ros::NodeHandle& getMTPrivateNodeHandle() const;
202 
203  ros::CallbackQueueInterface& getSTCallbackQueue() const;
204  ros::CallbackQueueInterface& getMTCallbackQueue() const;
205 
206 
207  // Internal storage;
208 private:
209  bool inited_;
210 
211  std::string nodelet_name_;
212 
213  NodeHandlePtr nh_;
214  NodeHandlePtr private_nh_;
215  NodeHandlePtr mt_nh_;
216  NodeHandlePtr mt_private_nh_;
217  V_string my_argv_;
218  M_string remapping_args_;
219 
220  // Method to be overridden by subclass when starting up.
221  virtual void onInit() = 0;
222 
223  // Public API used for launching
224 public:
226  Nodelet();
227 
233  void init(const std::string& name, const M_string& remapping_args, const V_string& my_argv,
234  ros::CallbackQueueInterface* st_queue = NULL,
235  ros::CallbackQueueInterface* mt_queue = NULL);
236 
237  virtual ~Nodelet();
238 };
239 
240 }
241 #endif //NODELET_NODELET_H
const V_string & getMyArgv() const
Definition: nodelet.h:195
NodeHandlePtr mt_nh_
Definition: nodelet.h:215
Base class for all exceptions thrown by nodelet.
Definition: exception.h:39
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const std::string & getName() const
Definition: nodelet.h:190
NodeHandlePtr private_nh_
Definition: nodelet.h:214
boost::shared_ptr< NodeHandle > NodeHandlePtr
M_string remapping_args_
Definition: nodelet.h:218
std::string nodelet_name_
Definition: nodelet.h:211
std::map< std::string, std::string > M_string
std::vector< std::string > V_string
V_string my_argv_
Definition: nodelet.h:217
#define NODELETLIB_DECL
Definition: nodeletdecl.h:40
UninitializedException(const std::string &method_name)
Definition: nodelet.h:173
NodeHandlePtr nh_
Definition: nodelet.h:213
NodeHandlePtr mt_private_nh_
Definition: nodelet.h:216
const M_string & getRemappingArgs() const
Definition: nodelet.h:196
std::string getSuffixedName(const std::string &suffix) const
Definition: nodelet.h:191


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Sat Jul 18 2020 03:17:53