Print weird laser readings of a LaserScan topic

print_weird_laser_readings.py

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
import numpy as np

HEADER = '\033[95m'
OKBLUE = '\033[94m'
OKGREEN = '\033[92m'
WARNING = '\033[93m'
FAIL = '\033[91m'
ENDC = '\033[0m'

def is_zero(value):
    return value > (0.0 - np.finfo(type(value)).eps) and value < (0.0 + np.finfo(type(value)).eps)

class LaserAlert():
    """Subscribe to a laser scan topic and print out weird readings summary.
    Checks for zeros, nans, -inf, +inf, lower than range_min, bigger than range_max."""
    def __init__(self):
        self.sub = rospy.Subscriber('/scan', LaserScan, self.scan_cb)
        rospy.loginfo("Subscribed to: '" + self.sub.resolved_name + "' LaserScan topic.")

    def scan_cb(self, data):
        """
        :type data: LaserScan
        """
        zeros_count = 0
        inf_count = 0
        neg_inf_count = 0
        nan_count = 0
        lower_than_min_count = 0
        bigger_than_max_count = 0
        negative_count = 0

        minimum_scans = []
        min_val_scan = 0.00999999977648  # 0.003000000026077032 is a reading that appears with SICK

        for reading in data.ranges:
            if is_zero(reading):
                zeros_count += 1
            if np.isnan(reading):
                nan_count += 1
            if reading < 0.0 and reading != float("-Inf"):
                negative_count += 1
                print FAIL + "Found a negative value of: " + str(reading) + ENDC
            if reading < data.range_min and reading != float("-Inf") and not np.isnan(reading) and not is_zero(reading):
                print FAIL + "Found " + str(reading) + " lower than range_min! (" + str(data.range_min) + ")" + ENDC
                lower_than_min_count += 1
            if reading == float('inf'):
                inf_count += 1
            if reading == float('-inf'):
                neg_inf_count += 1
            if reading > data.range_max and reading != float("Inf") and not np.isnan(reading):
                bigger_than_max_count += 1
                print FAIL + "Found " + str(reading) + " bigger than range_max! (" + str(data.range_max) + ")" + ENDC

            if reading < min_val_scan and reading != float('-Inf') and not np.isnan(reading) and not is_zero(reading):
                minimum_scans.append(reading)

        if zeros_count > 0:
            print WARNING + "There were " + str(zeros_count) + " zeros (0.0)" + ENDC
        if inf_count > 0:
            print WARNING + "There were " + str(inf_count) + " inf's" + ENDC
        if neg_inf_count > 0:
            print WARNING + "There were " + str(neg_inf_count) + " -inf's" + ENDC
        if nan_count > 0:
            print WARNING + "There were " + str(nan_count) + " NaNs" + ENDC
        if lower_than_min_count > 0:
            print FAIL + str(lower_than_min_count) + " lower than range_min readings in one scan" + ENDC
        if bigger_than_max_count > 0:
            print FAIL + str(bigger_than_max_count) + " bigger than range_max readings in one scan" + ENDC
        if negative_count > 0:
            print FAIL + str(negative_count) + " negative readings in one scan" + ENDC

        if len(minimum_scans) > 0:
            print WARNING + "There were " + str(len(minimum_scans)) + " readings closer than " + str(min_val_scan) + ", they are: \n" + str(minimum_scans) + ENDC


        print "\n\n"



if __name__ == '__main__':
    rospy.init_node('laser_alert', anonymous=True)
    la = LaserAlert()
    rospy.spin()