validate_floats.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, 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 RVIZ_VALIDATE_FLOAT_H
31 #define RVIZ_VALIDATE_FLOAT_H
32 
33 #include <cmath>
34 
35 #include <geometry_msgs/PointStamped.h>
36 #include <geometry_msgs/Point32.h>
37 #include <geometry_msgs/Vector3.h>
38 #include <geometry_msgs/PoseStamped.h>
39 #include <geometry_msgs/Twist.h>
40 #include <std_msgs/ColorRGBA.h>
41 #include <OgreVector3.h>
42 #include <OgreQuaternion.h>
43 
44 #include <boost/array.hpp>
45 
46 namespace rviz
47 {
48 
49 inline bool validateFloats(float val)
50 {
51  return !(std::isnan(val) || std::isinf(val));
52 }
53 
54 inline bool validateFloats(double val)
55 {
56  return !(std::isnan(val) || std::isinf(val));
57 }
58 
59 inline bool validateFloats(const Ogre::Vector3& vec)
60 {
61  bool valid = true;
62  valid = valid && validateFloats(vec.x);
63  valid = valid && validateFloats(vec.y);
64  valid = valid && validateFloats(vec.z);
65  return valid;
66 }
67 
68 inline bool validateFloats(const Ogre::Quaternion& quat)
69 {
70  bool valid = true;
71  valid = valid && validateFloats(quat.x);
72  valid = valid && validateFloats(quat.y);
73  valid = valid && validateFloats(quat.z);
74  valid = valid && validateFloats(quat.w);
75  return valid;
76 }
77 
78 inline bool validateFloats(const geometry_msgs::Point& msg)
79 {
80  bool valid = true;
81  valid = valid && validateFloats(msg.x);
82  valid = valid && validateFloats(msg.y);
83  valid = valid && validateFloats(msg.z);
84  return valid;
85 }
86 
87 inline bool validateFloats(const geometry_msgs::Point32& msg)
88 {
89  bool valid = true;
90  valid = valid && validateFloats(msg.x);
91  valid = valid && validateFloats(msg.y);
92  valid = valid && validateFloats(msg.z);
93  return valid;
94 }
95 
96 inline bool validateFloats(const geometry_msgs::Vector3& msg)
97 {
98  bool valid = true;
99  valid = valid && validateFloats(msg.x);
100  valid = valid && validateFloats(msg.y);
101  valid = valid && validateFloats(msg.z);
102  return valid;
103 }
104 
105 inline bool validateFloats(const geometry_msgs::Twist& twist)
106 {
107  bool valid = true;
108  valid = valid && validateFloats(twist.linear);
109  valid = valid && validateFloats(twist.angular);
110  return valid;
111 }
112 
113 inline bool validateFloats(const geometry_msgs::Quaternion& msg)
114 {
115  bool valid = true;
116  valid = valid && validateFloats(msg.x);
117  valid = valid && validateFloats(msg.y);
118  valid = valid && validateFloats(msg.z);
119  valid = valid && validateFloats(msg.w);
120  return valid;
121 }
122 
123 inline bool validateFloats(const std_msgs::ColorRGBA& msg)
124 {
125  bool valid = true;
126  valid = valid && validateFloats(msg.r);
127  valid = valid && validateFloats(msg.g);
128  valid = valid && validateFloats(msg.b);
129  valid = valid && validateFloats(msg.a);
130  return valid;
131 }
132 
133 inline bool validateFloats(const geometry_msgs::PointStamped& msg)
134 {
135  return validateFloats(msg.point);
136 }
137 
138 inline bool validateFloats(const geometry_msgs::Pose& msg)
139 {
140  bool valid = true;
141  valid = valid && validateFloats(msg.position);
142  valid = valid && validateFloats(msg.orientation);
143  return valid;
144 }
145 
146 inline bool validateFloats(const geometry_msgs::PoseStamped& msg)
147 {
148  return validateFloats(msg.pose);
149 }
150 
151 template<typename T>
152 inline bool validateFloats(const std::vector<T>& vec)
153 {
154  typedef std::vector<T> VecType;
155  typename VecType::const_iterator it = vec.begin();
156  typename VecType::const_iterator end = vec.end();
157  for (; it != end; ++it)
158  {
159  if (!validateFloats(*it))
160  {
161  return false;
162  }
163  }
164 
165  return true;
166 }
167 
168 template<typename T, size_t N>
169 inline bool validateFloats(const boost::array<T, N>& arr)
170 {
171  typedef boost::array<T, N> ArrType;
172  typename ArrType::const_iterator it = arr.begin();
173  typename ArrType::const_iterator end = arr.end();
174  for (; it != end; ++it)
175  {
176  if (!validateFloats(*it))
177  {
178  return false;
179  }
180  }
181 
182  return true;
183 }
184 
185 } // namespace rviz
186 
187 #endif // RVIZ_VALIDATE_FLOAT_H
bool validateFloats(const sensor_msgs::CameraInfo &msg)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42