Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef RVIZ_VALIDATE_FLOAT_H
00031 #define RVIZ_VALIDATE_FLOAT_H
00032
00033 #include <cmath>
00034
00035 #include <geometry_msgs/PointStamped.h>
00036 #include <geometry_msgs/Point32.h>
00037 #include <geometry_msgs/Vector3.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <geometry_msgs/Twist.h>
00040 #include <std_msgs/ColorRGBA.h>
00041 #include <OgreVector3.h>
00042 #include <OgreQuaternion.h>
00043
00044 #include <boost/array.hpp>
00045
00046 namespace rviz
00047 {
00048
00049 inline bool validateFloats(float val)
00050 {
00051 return !(std::isnan(val) || std::isinf(val));
00052 }
00053
00054 inline bool validateFloats(double val)
00055 {
00056 return !(std::isnan(val) || std::isinf(val));
00057 }
00058
00059 inline bool validateFloats(const Ogre::Vector3& vec)
00060 {
00061 bool valid = true;
00062 valid = valid && validateFloats(vec.x);
00063 valid = valid && validateFloats(vec.y);
00064 valid = valid && validateFloats(vec.z);
00065 return valid;
00066 }
00067
00068 inline bool validateFloats(const Ogre::Quaternion& quat)
00069 {
00070 bool valid = true;
00071 valid = valid && validateFloats(quat.x);
00072 valid = valid && validateFloats(quat.y);
00073 valid = valid && validateFloats(quat.z);
00074 valid = valid && validateFloats(quat.w);
00075 return valid;
00076 }
00077
00078 inline bool validateFloats(const geometry_msgs::Point& msg)
00079 {
00080 bool valid = true;
00081 valid = valid && validateFloats(msg.x);
00082 valid = valid && validateFloats(msg.y);
00083 valid = valid && validateFloats(msg.z);
00084 return valid;
00085 }
00086
00087 inline bool validateFloats(const geometry_msgs::Point32& msg)
00088 {
00089 bool valid = true;
00090 valid = valid && validateFloats(msg.x);
00091 valid = valid && validateFloats(msg.y);
00092 valid = valid && validateFloats(msg.z);
00093 return valid;
00094 }
00095
00096 inline bool validateFloats(const geometry_msgs::Vector3& msg)
00097 {
00098 bool valid = true;
00099 valid = valid && validateFloats(msg.x);
00100 valid = valid && validateFloats(msg.y);
00101 valid = valid && validateFloats(msg.z);
00102 return valid;
00103 }
00104
00105 inline bool validateFloats(const geometry_msgs::Twist& twist)
00106 {
00107 bool valid = true;
00108 valid = valid && validateFloats(twist.linear);
00109 valid = valid && validateFloats(twist.angular);
00110 return valid;
00111 }
00112
00113 inline bool validateFloats(const geometry_msgs::Quaternion& msg)
00114 {
00115 bool valid = true;
00116 valid = valid && validateFloats(msg.x);
00117 valid = valid && validateFloats(msg.y);
00118 valid = valid && validateFloats(msg.z);
00119 valid = valid && validateFloats(msg.w);
00120 return valid;
00121 }
00122
00123 inline bool validateFloats(const std_msgs::ColorRGBA& msg)
00124 {
00125 bool valid = true;
00126 valid = valid && validateFloats(msg.r);
00127 valid = valid && validateFloats(msg.g);
00128 valid = valid && validateFloats(msg.b);
00129 valid = valid && validateFloats(msg.a);
00130 return valid;
00131 }
00132
00133 inline bool validateFloats(const geometry_msgs::PointStamped& msg)
00134 {
00135 return validateFloats(msg.point);
00136 }
00137
00138 inline bool validateFloats(const geometry_msgs::Pose& msg)
00139 {
00140 bool valid = true;
00141 valid = valid && validateFloats(msg.position);
00142 valid = valid && validateFloats(msg.orientation);
00143 return valid;
00144 }
00145
00146 inline bool validateFloats(const geometry_msgs::PoseStamped& msg)
00147 {
00148 return validateFloats(msg.pose);
00149 }
00150
00151 template<typename T>
00152 inline bool validateFloats(const std::vector<T>& vec)
00153 {
00154 typedef std::vector<T> VecType;
00155 typename VecType::const_iterator it = vec.begin();
00156 typename VecType::const_iterator end = vec.end();
00157 for (; it != end; ++it)
00158 {
00159 if (!validateFloats(*it))
00160 {
00161 return false;
00162 }
00163 }
00164
00165 return true;
00166 }
00167
00168 template<typename T, size_t N>
00169 inline bool validateFloats(const boost::array<T, N>& arr)
00170 {
00171 typedef boost::array<T, N> ArrType;
00172 typename ArrType::const_iterator it = arr.begin();
00173 typename ArrType::const_iterator end = arr.end();
00174 for (; it != end; ++it)
00175 {
00176 if (!validateFloats(*it))
00177 {
00178 return false;
00179 }
00180 }
00181
00182 return true;
00183 }
00184
00185 }
00186
00187 #endif // RVIZ_VALIDATE_FLOAT_H