[Pythonclient/types] Add utility functions to check for NaN in Pose

PythonClient/airsim/types.py

@@ -1,6 +1,7 @@
 from __future__ import print_function
 import msgpackrpc #install as admin: pip install msgpack-rpc-python
 import numpy as np #pip install numpy
+import math
 
 class MsgpackMixin:
     def __repr__(self):
@@ -62,6 +63,9 @@ def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0):
     def nanVector3r():
         return Vector3r(np.nan, np.nan, np.nan)
 
+    def containsNan(self):
+        return (math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val))
+
     def __add__(self, other):
         return Vector3r(self.x_val + other.x_val, self.y_val + other.y_val, self.z_val + other.z_val)
 
@@ -122,6 +126,9 @@ def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0, w_val = 1.0):
     def nanQuaternionr():
         return Quaternionr(np.nan, np.nan, np.nan, np.nan)
 
+    def containsNan(self):
+        return (math.isnan(self.w_val) or math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val))
+
     def __add__(self, other):
         if type(self) == type(other):
             return Quaternionr( self.x_val+other.x_val, self.y_val+other.y_val, self.z_val+other.z_val, self.w_val+other.w_val )
@@ -207,6 +214,9 @@ def __init__(self, position_val = None, orientation_val = None):
     def nanPose():
         return Pose(Vector3r.nanVector3r(), Quaternionr.nanQuaternionr())
 
+    def containsNan(self):
+        return (self.position.containsNan() or self.orientation.containsNan())
+
 
 class CollisionInfo(MsgpackMixin):
     has_collided = False