Kindr
Kinematics and Dynamics for Robotics
assert_macros.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
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  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 #ifndef KINDR_ASSERT_MACROS_HPP_
29 #define KINDR_ASSERT_MACROS_HPP_
30 
31 
32 #include <stdexcept>
33 #include <sstream>
34 #include <typeinfo>
35 #include "source_file_pos.hpp"
36 
38 // (std::runtime_error should be top parent)
39 // adapted from ros/drivers/laser/hokuyo_driver/hokuyo.h
40 #define KINDR_DEFINE_EXCEPTION(exceptionName, exceptionParent) \
41  class exceptionName : public exceptionParent { \
42  public: \
43  exceptionName(const char * message) : exceptionParent(message) {} \
44  exceptionName(std::string const & message) : exceptionParent(message) {} \
45  virtual ~exceptionName() throw() {} \
46  };
47 
48 
49 namespace kindr {
50  namespace internal {
51 
52  template<typename KINDR_EXCEPTION_T>
53  inline void kindr_throw_exception(std::string const & exceptionType, kindr::internal::source_file_pos sfp, std::string const & message)
54  {
55  std::stringstream kindr_assert_stringstream;
56 #ifdef _WIN32
57  // I have no idea what broke this on Windows but it doesn't work with the << operator.
58  kindr_assert_stringstream << exceptionType << sfp.toString() << " " << message;
59 #else
60  kindr_assert_stringstream << exceptionType << sfp.toString() << std::string{" "} << message;
61 #endif
62  throw(KINDR_EXCEPTION_T(kindr_assert_stringstream.str()));
63  }
64 
65  template<typename KINDR_EXCEPTION_T>
66  inline void kindr_throw_exception(std::string const & exceptionType, std::string const & function, std::string const & file,
67  int line, std::string const & message)
68  {
69  kindr_throw_exception<KINDR_EXCEPTION_T>(exceptionType, kindr::internal::source_file_pos(function,file,line),message);
70  }
71 
72 
73  } // namespace internal
74 
75  template<typename KINDR_EXCEPTION_T>
76  inline void kindr_assert_throw(bool assert_condition, std::string message, kindr::internal::source_file_pos sfp) {
77  if(!assert_condition)
78  {
79  internal::kindr_throw_exception<KINDR_EXCEPTION_T>("", sfp,message);
80  }
81  }
82 
83 
84 
85 } // namespace kindr
86 
87 
88 
89 
90 #define KINDR_THROW(exceptionType, message) { \
91  std::stringstream kindr_assert_stringstream; \
92  kindr_assert_stringstream << message; \
93  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, kindr_assert_stringstream.str()); \
94  }
95 
96 
97 #define KINDR_THROW_SFP(exceptionType, SourceFilePos, message){ \
98  std::stringstream kindr_assert_stringstream; \
99  kindr_assert_stringstream << message; \
100  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", SourceFilePos, kindr_assert_stringstream.str()); \
101  }
102 
103 #define KINDR_ASSERT_TRUE(exceptionType, condition, message) \
104  if(!(condition)) \
105  { \
106  std::stringstream kindr_assert_stringstream; \
107  kindr_assert_stringstream << "assert(" << #condition << ") failed: " << message; \
108  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, kindr_assert_stringstream.str()); \
109  }
110 
111 #define KINDR_ASSERT_FALSE(exceptionType, condition, message) \
112  if((condition)) \
113  { \
114  std::stringstream kindr_assert_stringstream; \
115  kindr_assert_stringstream << "assert( not " << #condition << ") failed: " << message; \
116  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, kindr_assert_stringstream.str()); \
117  }
118 
119 
120 
121 #define KINDR_ASSERT_GE_LT(exceptionType, value, lowerBound, upperBound, message) \
122  if((value) < (lowerBound) || (value) >= (upperBound)) \
123  { \
124  std::stringstream kindr_assert_stringstream; \
125  kindr_assert_stringstream << "assert(" << #lowerBound << " <= " << #value << " < " << #upperBound << ") failed [" << (lowerBound) << " <= " << (value) << " < " << (upperBound) << "]: " << message; \
126  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
127  }
128 
129 
130 
131 #define KINDR_ASSERT_LT(exceptionType, value, upperBound, message) \
132  if((value) >= (upperBound)) \
133  { \
134  std::stringstream kindr_assert_stringstream; \
135  kindr_assert_stringstream << "assert(" << #value << " < " << #upperBound << ") failed [" << (value) << " < " << (upperBound) << "]: " << message; \
136  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
137  }
138 
139 #define KINDR_ASSERT_GE(exceptionType, value, lowerBound, message) \
140  if((value) < (lowerBound)) \
141  { \
142  std::stringstream kindr_assert_stringstream; \
143  kindr_assert_stringstream << "assert(" << #value << " >= " << #lowerBound << ") failed [" << (value) << " >= " << (lowerBound) << "]: " << message; \
144  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
145  }
146 
147 
148 
149 #define KINDR_ASSERT_LE(exceptionType, value, upperBound, message) \
150  if((value) > (upperBound)) \
151  { \
152  std::stringstream kindr_assert_stringstream; \
153  kindr_assert_stringstream << "assert(" << #value << " <= " << #upperBound << ") failed [" << (value) << " <= " << (upperBound) << "]: " << message; \
154  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
155  }
156 
157 #define KINDR_ASSERT_GT(exceptionType, value, lowerBound, message) \
158  if((value) <= (lowerBound)) \
159  { \
160  std::stringstream kindr_assert_stringstream; \
161  kindr_assert_stringstream << "assert(" << #value << " > " << #lowerBound << ") failed [" << (value) << " > " << (lowerBound) << "]: " << message; \
162  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
163  }
164 
165 
166 
167 #define KINDR_ASSERT_EQ(exceptionType, value, testValue, message) \
168  if((value) != (testValue)) \
169  { \
170  std::stringstream kindr_assert_stringstream; \
171  kindr_assert_stringstream << "assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << "]: " << message; \
172  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
173  }
174 
175 #define KINDR_ASSERT_NE(exceptionType, value, testValue, message) \
176  if((value) == (testValue)) \
177  { \
178  std::stringstream kindr_assert_stringstream; \
179  kindr_assert_stringstream << "assert(" << #value << " != " << #testValue << ") failed [" << (value) << " != " << (testValue) << "]: " << message; \
180  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
181  }
182 
183 #define KINDR_ASSERT_NEAR(exceptionType, value, testValue, abs_error, message) \
184  if(!(fabs((testValue) - (value)) <= fabs(abs_error))) \
185  { \
186  std::stringstream kindr_assert_stringstream; \
187  kindr_assert_stringstream << "assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << " (" << fabs((testValue) - (value)) << " > " << fabs(abs_error) << ")]: " << message; \
188  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
189  }
190 
191 
192 
193 #ifndef NDEBUG
194 
195 #define KINDR_THROW_DBG(exceptionType, message){ \
196  std::stringstream kindr_assert_stringstream; \
197  kindr_assert_stringstream << message; \
198  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, kindr_assert_stringstream.str()); \
199  }
200 
201 
202 
203 #define KINDR_ASSERT_TRUE_DBG(exceptionType, condition, message) \
204  if(!(condition)) \
205  { \
206  std::stringstream kindr_assert_stringstream; \
207  kindr_assert_stringstream << "debug assert(" << #condition << ") failed: " << message; \
208  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, kindr_assert_stringstream.str()); \
209  }
210 
211 #define KINDR_ASSERT_FALSE_DBG(exceptionType, condition, message) \
212  if((condition)) \
213  { \
214  std::stringstream kindr_assert_stringstream; \
215  kindr_assert_stringstream << "debug assert( not " << #condition << ") failed: " << message; \
216  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, kindr_assert_stringstream.str()); \
217  }
218 
219 
220 #define KINDR_ASSERT_DBG_RE( condition, message) KINDR_ASSERT_DBG(std::runtime_error, condition, message)
221 
222 #define KINDR_ASSERT_GE_LT_DBG(exceptionType, value, lowerBound, upperBound, message) \
223  if((value) < (lowerBound) || (value) >= (upperBound)) \
224  { \
225  std::stringstream kindr_assert_stringstream; \
226  kindr_assert_stringstream << "debug assert(" << #lowerBound << " <= " << #value << " < " << #upperBound << ") failed [" << (lowerBound) << " <= " << (value) << " < " << (upperBound) << "]: " << message; \
227  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
228  }
229 
230 
231 
232 #define KINDR_ASSERT_LT_DBG(exceptionType, value, upperBound, message) \
233  if((value) >= (upperBound)) \
234  { \
235  std::stringstream kindr_assert_stringstream; \
236  kindr_assert_stringstream << "debug assert(" << #value << " < " << #upperBound << ") failed [" << (value) << " < " << (upperBound) << "]: " << message; \
237  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
238  }
239 
240 
241 
242 #define KINDR_ASSERT_GE_DBG(exceptionType, value, lowerBound, message) \
243  if((value) < (lowerBound)) \
244  { \
245  std::stringstream kindr_assert_stringstream; \
246  kindr_assert_stringstream << "debug assert(" << #value << " >= " << #lowerBound << ") failed [" << (value) << " >= " << (lowerBound) << "]: " << message; \
247  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
248  }
249 
250 
251 
252 #define KINDR_ASSERT_LE_DBG(exceptionType, value, upperBound, message) \
253  if((value) > (upperBound)) \
254  { \
255  std::stringstream kindr_assert_stringstream; \
256  kindr_assert_stringstream << "debug assert(" << #value << " <= " << #upperBound << ") failed [" << (value) << " <= " << (upperBound) << "]: " << message; \
257  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
258  }
259 
260 #define KINDR_ASSERT_GT_DBG(exceptionType, value, lowerBound, message) \
261  if((value) <= (lowerBound)) \
262  { \
263  std::stringstream kindr_assert_stringstream; \
264  kindr_assert_stringstream << "debug assert(" << #value << " > " << #lowerBound << ") failed [" << (value) << " > " << (lowerBound) << "]: " << message; \
265  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
266  }
267 
268 
269 
270 #define KINDR_ASSERT_EQ_DBG(exceptionType, value, testValue, message) \
271  if((value) != (testValue)) \
272  { \
273  std::stringstream kindr_assert_stringstream; \
274  kindr_assert_stringstream << "debug assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << "]: " << message; \
275  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
276  }
277 
278 
279 #define KINDR_ASSERT_NE_DBG(exceptionType, value, testValue, message) \
280  if((value) == (testValue)) \
281  { \
282  std::stringstream kindr_assert_stringstream; \
283  kindr_assert_stringstream << "debug assert(" << #value << " != " << #testValue << ") failed [" << (value) << " != " << (testValue) << "]: " << message; \
284  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
285  }
286 
287 
288 
289 #define KINDR_ASSERT_NEAR_DBG(exceptionType, value, testValue, abs_error, message) \
290  if(!(fabs((testValue) - (value)) <= fabs(abs_error))) \
291  { \
292  std::stringstream kindr_assert_stringstream; \
293  kindr_assert_stringstream << "debug assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << " (" << fabs((testValue) - (value)) << " > " << fabs(abs_error) << ")]: " << message; \
294  kindr::internal::kindr_throw_exception<exceptionType>("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,kindr_assert_stringstream.str()); \
295  }
296 
297 
298 #define KINDR_OUT(X) std::cout << #X << ": " << (X) << std::endl
299 
300 #else
301 
302 #define KINDR_OUT(X)
303 #define KINDR_THROW_DBG(exceptionType, message)
304 #define KINDR_ASSERT_TRUE_DBG(exceptionType, condition, message)
305 #define KINDR_ASSERT_FALSE_DBG(exceptionType, condition, message)
306 #define KINDR_ASSERT_GE_LT_DBG(exceptionType, value, lowerBound, upperBound, message)
307 #define KINDR_ASSERT_LT_DBG(exceptionType, value, upperBound, message)
308 #define KINDR_ASSERT_GT_DBG(exceptionType, value, lowerBound, message)
309 #define KINDR_ASSERT_LE_DBG(exceptionType, value, upperBound, message)
310 #define KINDR_ASSERT_GE_DBG(exceptionType, value, lowerBound, message)
311 #define KINDR_ASSERT_NE_DBG(exceptionType, value, testValue, message)
312 #define KINDR_ASSERT_EQ_DBG(exceptionType, value, testValue, message)
313 #define KINDR_ASSERT_NEAR_DBG(exceptionType, value, testValue, abs_error, message)
314 #endif
315 
316 #endif /* KINDR_ASSERT_MACROS_HPP_ */
void kindr_assert_throw(bool assert_condition, std::string message, kindr::internal::source_file_pos sfp)