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'


SCAN_TOPIC = "/scan"

class LaserAlert():
    """"""
    def __init__(self):
        self.sub = rospy.Subscriber(SCAN_TOPIC, LaserScan, self.scan_cb)

    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.10

        #print str(len(data.ranges)) + " readings in one scan."
        total_readings = len(data.ranges)

        for reading in data.ranges:
            if reading == 0.0:
                zeros_count += 1
            if reading == float("NaN"):
                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"):
                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"):
                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'):
                minimum_scans.append(reading)

        if zeros_count > 0:
            print WARNING + "There were " + str(zeros_count) + " zeros (0.0) of " + str(total_readings) + " (" + str(round(float(zeros_count)/float(total_readings)*100.0, 2)) + "%)" + ENDC
        if inf_count > 0:
            print WARNING + "There were " + str(inf_count) + " inf's of " + str(total_readings) + " (" + str(round(float(inf_count)/float(total_readings)*100.0, 2)) + "%)" + ENDC
        if neg_inf_count > 0:
            print WARNING + "There were " + str(neg_inf_count) + " -inf's of " + str(total_readings) + " (" + str(round(float(neg_inf_count)/float(total_readings)*100.0, 2)) + "%)"  + ENDC
        if nan_count > 0:
            print WARNING + "There were " + str(nan_count) + " NaNs of " + str(total_readings) + " (" + str(round(float(nan_count)/float(total_readings)*100.0, 2)) + "%)"  + ENDC
        if lower_than_min_count > 0:
            print FAIL + str(lower_than_min_count) + " lower than range_min readings in one scan of " + str(total_readings) + " (" + str(round(float(lower_than_min_count)/float(total_readings)*100.0, 2)) + "%)"  + ENDC
        if bigger_than_max_count > 0:
            print FAIL + str(bigger_than_max_count) + " bigger than range_max readings in one scan of " + str(total_readings) + " (" + str(round(float(bigger_than_max_count)/float(total_readings)*100.0, 2)) + "%)"  + ENDC
        if negative_count > 0:
            print FAIL + str(negative_count) + " negative readings in one scan of " + str(total_readings) + " (" + str(round(float(negative_count)/float(total_readings)*100.0, 2)) + "%)"  + 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






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