Script that subscribes to a LaserScan topic and shows the problematic readings it may show (NaNs, Infs, closer than range_min, further away than range_max, and others...)

print_weird_laser_readings_with_markers.py

#!/usr/bin/env python

"""
This script subscribes to a LaserScan topic, by default '/scan',
and shows in the terminal statistics of 'weird' laser readings like:
zeros, nans, -inf, +inf, lower than range_min, bigger than range_max
Which are commonly given by different laser manufacturers.

It also publishes a MarkerArray for Rviz at '/scan_problems_marker_array'
which shows the different laser lines that give problems in real time
with the name of the problem.

The shell output looks like:
Bad readings: 117/513, that's 22.807%
There were 106 inf's
There were 5 NaNs

Rviz looks like:
http://i.imgur.com/Dd1haI9.png

Author: Sammy Pfeiffer
"""


from __future__ import division
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Point
from visualization_msgs.msg import MarkerArray, Marker
from std_msgs.msg import ColorRGBA
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)

def create_reading_markers(reading_type, reading_id, msg, marker_id):
    # zeros, nans, -inf, +inf, lower than range_min, bigger than range_max
    m = Marker()
    m.header.frame_id = msg.header.frame_id
    m.type = m.ARROW

    if reading_type == 'zero':
        m.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
    elif reading_type == 'nan':
        m.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)
    elif reading_type == '-inf':
        m.color = ColorRGBA(0.0, 0.0, 1.0, 1.0)
    elif reading_type == '+inf':
        m.color = ColorRGBA(0.5, 0.0, 1.0, 1.0)
    elif reading_type == 'lower range_min':
        m.color = ColorRGBA(0.0, 1.0, 0.0, 1.0)
    elif reading_type == 'bigger range_max':
        m.color = ColorRGBA(0.5, 0.25, 0.25, 1.0)
    elif reading_type == 'negative':
        m.color = ColorRGBA(0.3, 0.1, 0.7, 1.0)
    elif reading_type == 'weird tiny value':
        m.color = ColorRGBA(0.0, 0.4, 0.7, 1.0)

    m.points.append(Point(0.0, 0.0, 0.0))
    angle = msg.angle_min + msg.angle_increment * reading_id
    distance = msg.range_max
    m.ns = reading_type
    x = np.cos(angle) * distance
    y = np.sin(angle) * distance
    m.points.append(Point(x, y, 0.0))
    m.scale.x = 0.05
    m.scale.y = 0.1
    m.scale.z = 0.2
    m.id = marker_id
    m.lifetime = rospy.Duration(0.1)

    m_text = Marker()
    m_text.header.frame_id = msg.header.frame_id
    m_text.type = m.TEXT_VIEW_FACING
    m_text.color = m.color
    m_text.text = reading_type
    m_text.scale.z = 0.3
    m_text.pose.position = m.points[1]
    m_text.id = marker_id + 1
    m_text.ns = reading_type
    m_text.lifetime = rospy.Duration(0.1)

    return m, m_text


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, queue_size=5)
        rospy.loginfo("Subscribed to: '" + self.sub.resolved_name + "' LaserScan topic.")
        self.marker_array = MarkerArray()
        self.marker_id = 99
        self.pub = rospy.Publisher('/scan_problems_marker_array', MarkerArray, queue_size=5)

    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

        this_scan_ids = 99

        for idx, reading in enumerate(data.ranges):
            if is_zero(reading):
                zeros_count += 1
                m, m_text = create_reading_markers('zero', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            if np.isnan(reading):
                nan_count += 1
                m, m_text = create_reading_markers('nan', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            if reading < 0.0 and reading != float("-Inf"):
                negative_count += 1
                print FAIL + "Found a negative value of: " + str(reading) + ENDC
                m, m_text = create_reading_markers('negative', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            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
                m, m_text = create_reading_markers('lower range_min', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            if reading == float('inf'):
                inf_count += 1
                m, m_text = create_reading_markers('+inf', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            if reading == float('-inf'):
                neg_inf_count += 1
                m, m_text = create_reading_markers('-inf', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            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
                m, m_text = create_reading_markers('bigger range_max', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

            if reading < min_val_scan and reading != float('-Inf') and not np.isnan(reading) and not is_zero(reading):
                minimum_scans.append(reading)
                m, m_text = create_reading_markers('weird tiny value', idx, data, this_scan_ids)
                this_scan_ids += 2
                self.marker_array.markers.append(m)
                self.marker_array.markers.append(m_text)

        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


        bad_readings = zeros_count + inf_count + neg_inf_count + nan_count + lower_than_min_count + bigger_than_max_count + negative_count
        print "\n\nBad readings: " + str(bad_readings) + "/" + str(len(data.ranges)) + ", that's " + str(round(bad_readings/len(data.ranges) * 100.0, 3)) + "%"
        self.pub.publish(self.marker_array)
        self.marker_array = MarkerArray()



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