[jsk_fetch_startup] add nan checking process to odometry_transformer.py

jsk_fetch_robot/jsk_fetch_startup/scripts/odometry_transformer.py

@@ -9,6 +9,7 @@
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import TransformStamped
 
+import math
 
 class OdometryTransformer(object):
 
@@ -78,6 +79,22 @@ def __init__(self):
 
     def cb_odometry(self, msg):
 
+        # Nan check
+        if math.isnan( msg.pose.pose.position.x ) or
+           math.isnan( msg.pose.pose.position.y ) or
+           math.isnan( msg.pose.pose.position.z ) or
+           math.isnan( msg.pose.pose.orientation.x ) or
+           math.isnan( msg.pose.pose.orientation.y ) or
+           math.isnan( msg.pose.pose.orientation.z ) or
+           math.isnan( msg.twist.twist.linear.x ) or
+           math.isnan( msg.twist.twist.linear.y ) or
+           math.isnan( msg.twist.twist.linear.z ) or
+           math.isnan( msg.twist.twist.angular.x ) or
+           math.isnan( msg.twist.twist.angular.y ) or
+           math.isnan( msg.twist.twist.angular.z ):
+            rospy.logwarn('Recieved an odom message with nan values')
+            return
+
         pos_t265_odombased = np.array([
             msg.pose.pose.position.x,
             msg.pose.pose.position.y,