test_scan.py

Go to the documentation of this file.

#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the Willow Garage nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#

## Gazebo test cameras validation 

PKG = 'pr2_gazebo'
NAME = 'test_scan'

import math
import roslib
roslib.load_manifest(PKG)


import sys, unittest
import os, os.path, threading, time
import rospy, rostest
from sensor_msgs.msg import LaserScan

TEST_DURATION  = 25
ERROR_TOL      = 0.05
FAIL_COUNT_TOL = 10

TARGET_RANGES = [
10.0, 10.0, 10.0, 9.99573993683, 9.99354457855, 10.0, 9.99495887756, 
9.99537754059, 9.99530220032, 9.99065494537, 9.99572181702, 10.0, 9.99661064148, 10.0, 
9.9931268692, 10.0, 9.99523544312, 9.99910163879, 10.0, 9.99757385254, 10.0, 
10.0, 10.0, 9.98721313477, 10.0, 10.0, 9.99776268005, 9.98858833313, 
9.99840354919, 10.0, 9.9975643158, 10.0, 10.0, 9.9985332489, 9.9998550415, 
9.99693107605, 10.0, 9.99122810364, 10.0, 9.9965057373, 9.99392700195, 9.9958820343, 
9.99862098694, 10.0, 10.0, 10.0, 10.0, 10.0, 9.99697303772, 
10.0, 9.99373340607, 10.0, 9.98993110657, 10.0, 9.99621105194, 10.0, 
10.0, 9.99318027496, 9.99940681458, 9.99647140503, 10.0, 9.99137496948, 10.0, 
9.99066162109, 9.99819469452, 9.99851989746, 9.99762248993, 9.99655723572, 10.0, 9.9974155426, 
9.99393177032, 10.0, 10.0, 10.0, 10.0, 9.99311447144, 10.0, 
10.0, 9.99642086029, 9.99772071838, 9.99639415741, 10.0, 10.0, 9.99461174011, 
10.0, 9.99182224274, 9.99395465851, 10.0, 10.0, 9.99905490875, 10.0, 
9.9945936203, 10.0, 9.99512577057, 9.99258613586, 9.99485301971, 10.0, 9.99732017517, 
9.99589729309, 9.99851226807, 10.0, 10.0, 9.99726200104, 10.0, 10.0, 
10.0, 10.0, 9.99561405182, 9.9951543808, 10.0, 9.99977779388, 9.99529075623, 
10.0, 10.0, 9.99410533905, 9.98627853394, 9.9893321991, 10.0, 10.0, 
9.99817180634, 9.9997959137, 10.0, 9.9965801239, 10.0, 10.0, 9.99580669403, 
10.0, 10.0, 9.99091243744, 9.99701595306, 10.0, 9.9877243042, 9.99554157257, 
9.99643039703, 10.0, 10.0, 10.0, 10.0, 9.99929332733, 9.99739742279, 
9.99473190308, 10.0, 10.0, 10.0, 9.9960603714, 10.0, 9.99765872955, 
9.9982509613, 9.99988269806, 10.0, 10.0, 10.0, 10.0, 9.99551677704, 
9.99934577942, 9.99937820435, 9.99880981445, 10.0, 9.99725151062, 10.0, 10.0, 
10.0, 10.0, 9.99893283844, 9.99085235596, 10.0, 10.0, 9.99376106262, 
9.99191761017, 10.0, 9.99659919739, 9.99622631073, 9.99751281738, 10.0, 9.99464511871, 
9.99992275238, 10.0, 9.98847484589, 10.0, 10.0, 9.99902820587, 10.0, 
9.99963665009, 10.0, 9.99925041199, 9.99624538422, 9.99273204803, 10.0, 9.99636650085, 
10.0, 9.99062824249, 10.0, 9.99874019623, 10.0, 9.99279022217, 10.0, 
9.9953546524, 9.99644756317, 9.98907089233, 10.0, 10.0, 9.99982070923, 10.0, 
9.99953460693, 10.0, 10.0, 9.99766349792, 9.9923210144, 10.0, 10.0, 
10.0, 10.0, 9.99549865723, 9.9989824295, 9.98648643494, 9.99997234344, 10.0, 
10.0, 10.0, 9.99065971375, 9.99425029755, 10.0, 9.99474906921, 10.0, 
10.0, 10.0, 10.0, 9.99846553802, 9.99258041382, 9.99472618103, 9.99771308899, 
10.0, 10.0, 9.99508285522, 9.99805355072, 9.99019050598, 9.9955291748, 2.03685235977, 
2.04421663284, 2.03210663795, 2.0194671154, 2.0155646801, 1.99427628517, 2.00035619736, 1.98695075512, 
1.97852289677, 1.96805429459, 1.9588958025, 1.95648872852, 1.94692718983, 1.93678343296, 1.91842794418, 
1.92827701569, 1.91704964638, 1.91358006001, 1.92673146725, 1.94037222862, 9.99822807312, 10.0, 
10.0, 10.0, 2.14445352554, 2.08714962006, 2.05095148087, 2.02395749092, 2.00428080559, 
1.97976839542, 1.95980763435, 1.9413497448, 1.93820095062, 1.91665804386, 1.92057430744, 1.9003251791, 
1.88783442974, 1.8934186697, 1.8741286993, 1.86705768108, 1.85493171215, 1.85695803165, 1.85333991051, 
1.83796668053, 1.84245407581, 1.84312713146, 1.84570860863, 1.82822322845, 1.83273553848, 1.83729255199, 
1.83289527893, 1.8334633112, 1.83232152462, 1.82959234715, 1.8262963295, 1.83373129368, 1.83350098133, 
1.83656823635, 1.83848130703, 1.83709740639, 1.85071253777, 1.84394586086, 1.86468243599, 1.85489320755, 
1.85955429077, 1.86517083645, 1.88544261456, 1.89780557156, 1.89888262749, 1.92238008976, 1.922524333, 
1.93953430653, 1.948564291, 1.96180999279, 1.98394215107, 1.99410378933, 2.02381777763, 2.05232095718, 
2.09428310394, 2.14106678963, 2.51629209518, 2.51145839691, 2.50856781006, 2.49926424026, 2.51138210297, 
2.50430202484, 2.50846076012, 2.52427816391, 2.52740097046, 2.5371067524, 2.54831123352, 2.56848597527, 
2.59808444977, 2.62111496925, 2.68968343735, 10.0, 9.99606513977, 10.0, 10.0, 
10.0, 10.0, 9.99753856659, 9.99373626709, 10.0, 2.72182154655, 2.7262597084, 
10.0, 9.99517822266, 9.99814987183, 9.99634075165, 10.0, 10.0, 10.0, 
10.0, 9.99925136566, 10.0, 10.0, 10.0, 10.0, 9.99726676941, 
10.0, 10.0, 1.80504393578, 1.76711213589, 1.73867917061, 1.7188462019, 1.706569314, 
1.69170439243, 1.67706441879, 1.66418457031, 1.6565977335, 1.64732301235, 1.64360272884, 1.63184809685, 
1.63481676579, 1.62855827808, 1.62778234482, 1.61982572079, 1.6105659008, 1.6124740839, 1.60954415798, 
1.6100025177, 1.60651493073, 1.59700119495, 1.60250425339, 1.60432767868, 1.602414608, 1.60677278042, 
1.61422848701, 1.61278867722, 1.61149096489, 1.61712217331, 1.61782002449, 1.63084983826, 1.63016283512, 
1.64222669601, 1.64433014393, 1.65826809406, 1.66858124733, 1.67725527287, 1.68793737888, 1.70181775093, 
1.71679854393, 1.74195897579, 1.76237237453, 1.80084371567, 10.0, 9.98779392242, 9.99672698975, 
10.0, 9.99886322021, 9.99884319305, 10.0, 10.0, 10.0, 10.0, 
9.99102210999, 10.0, 10.0, 9.9920835495, 9.99745178223, 10.0, 9.99134922028, 
10.0, 9.9976682663, 9.99491119385, 9.99395751953, 9.9923286438, 9.99616718292, 9.99658679962, 
9.98934459686, 10.0, 9.99688911438, 9.99960327148, 10.0, 9.99228954315, 10.0, 
9.99639987946, 9.99746799469, 9.99986839294, 9.99951839447, 9.99859428406, 10.0, 10.0, 
10.0, 9.99822998047, 9.99964332581, 9.99930763245, 9.99730014801, 9.99543762207, 9.99613285065, 
10.0, 9.99729442596, 10.0, 10.0, 10.0, 10.0, 9.99937438965, 
9.99928283691, 9.99720096588, 9.99948310852, 9.9946680069, 9.99388885498, 10.0, 10.0, 
10.0, 10.0, 9.99677371979, 9.99859809875, 10.0, 10.0, 9.99358463287, 
9.99317646027, 10.0, 9.99491786957, 10.0, 10.0, 9.99915504456, 10.0, 
9.99907207489, 10.0, 9.99767684937, 10.0, 10.0, 9.99805450439, 10.0, 
9.98858165741, 9.99410438538, 10.0, 9.99862289429, 10.0, 10.0, 10.0, 
9.99983596802, 9.99750804901, 10.0, 9.99136257172, 9.99489784241, 10.0, 9.99796485901, 
10.0, 10.0, 10.0, 9.99649906158, 9.99518966675, 10.0, 10.0, 
9.9939289093, 9.9948425293, 9.99791431427, 9.99170398712, 10.0, 9.99482250214, 9.99916267395, 
9.99699401855, 10.0, 9.99531364441, 10.0, 9.99355602264, 10.0, 9.99841976166, 
10.0, 10.0, 9.99886226654, 9.99558353424, 10.0, 10.0, 9.99983596802, 
10.0, 9.99355125427, 9.99629402161, 9.99861049652, 9.99837779999, 9.99851799011, 10.0, 
10.0, 9.99499320984, 9.99850845337, 9.99886894226, 10.0, 9.99392700195, 9.99863529205, 
9.99880886078, 9.99745464325, 9.99819660187, 9.99894714355, 10.0, 9.99813079834, 10.0, 
10.0, 9.99911022186, 10.0, 9.99262619019, 9.99907207489, 9.994389534, 10.0, 
9.99777507782, 9.99799919128, 9.99109649658, 9.99544143677, 9.99902153015, 10.0, 9.9965801239, 
10.0, 9.99814796448, 9.99841499329, 10.0, 9.9998960495, 10.0, 10.0, 
9.99292850494, 10.0, 9.99964237213, 10.0, 10.0, 9.99912929535, 9.99788856506, 
9.99653816223, 10.0, 10.0, 9.99705600739, 10.0, 10.0, 10.0, 
10.0, 9.99820232391, 10.0, 9.99996948242, 9.99960136414, 10.0, 10.0, 
10.0, 9.99853801727, 10.0, 10.0, 10.0, 9.99934387207, 10.0, 
10.0, 9.9871711731, 9.99130916595, 10.0, 10.0, 10.0, 9.99577236176, 
10.0, 10.0, 9.99879455566, 10.0, 10.0, 9.99161338806, 9.99415493011, 
10.0, 10.0, 10.0, 9.99282264709, 9.99699115753, 9.99679279327, 10.0, 
9.99227809906, 9.99905967712, 9.9926738739, 10.0, 10.0, 10.0, 9.99697971344, 
9.99261283875, 9.98644828796, 9.99855899811, 10.0, 9.99784469604, 10.0, 10.0, 
10.0, 9.99215602875, 9.98868179321, 9.99840354919, 9.99383068085, 10.0, 9.99576854706, 
9.99744510651, 10.0, 10.0, ]






TARGET_INTENSITIES = [
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 250.002059937, 500.003509521, 
500.005279541, 499.994689941, 500.009216309, 499.99597168, 499.988525391, 499.999633789, 499.992553711, 
499.995391846, 499.997558594, 500.000244141, 500.001342773, 499.990570068, 499.999420166, 499.998443604, 
500.003082275, 500.000579834, 500.011169434, 499.998931885, 249.997711182, 101.0, 101.0, 
101.0, 1500.00146484, 3000.0, 3000.00292969, 3000.00268555, 3000.00878906, 3000.00415039, 
3000.00097656, 2999.99853516, 3000.00805664, 3000.00146484, 3000.00415039, 2999.99633789, 2999.99609375, 
3000.00561523, 2999.99438477, 2999.99584961, 2999.99951172, 2999.99536133, 3000.00341797, 3000.00146484, 
2999.99536133, 2999.99633789, 3000.00048828, 3000.00268555, 2999.99243164, 3000.00415039, 2999.99853516, 
3000.00439453, 3000.00708008, 2999.99316406, 3000.00366211, 3000.0, 3000.0065918, 2999.99780273, 
3000.0, 2999.99121094, 2999.99902344, 3000.00195312, 2999.98901367, 2999.9934082, 2999.99951172, 
2999.99584961, 3000.00366211, 2999.99829102, 2999.99902344, 3000.00292969, 2999.99853516, 2999.99902344, 
2999.99658203, 2999.99829102, 2999.99658203, 3000.00146484, 3000.00195312, 3000.00878906, 2999.99414062, 
3000.00097656, 2499.99438477, 2000.00012207, 2000.00183105, 1999.98620605, 1999.99951172, 1999.99206543, 
2000.00280762, 1999.99804688, 2000.00195312, 1999.99401855, 1999.99853516, 1999.9901123, 1999.99731445, 
2000.00256348, 2000.00927734, 1000.00360107, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 1000.0012207, 1999.9967041, 1000.00085449, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 500.003265381, 999.998596191, 999.997680664, 999.991210938, 999.996032715, 1000.00134277, 
1000.00958252, 999.99786377, 1000.0055542, 999.996276855, 1000.00384521, 999.996765137, 1000.00292969, 
1000.00128174, 999.996154785, 1000.0032959, 1000.00061035, 999.99609375, 999.99621582, 999.996520996, 
1000.00500488, 999.997253418, 999.983886719, 1000.00042725, 999.998596191, 999.996887207, 999.992370605, 
1000.00213623, 1000.00799561, 999.997680664, 1000.00170898, 1000.00073242, 999.996398926, 1000.00830078, 
1000.00238037, 999.993774414, 999.993713379, 999.989990234, 999.996765137, 1000.00146484, 1000.00634766, 
999.990966797, 1000.00305176, 999.996704102, 499.996307373, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
101.0, 101.0, 101.0, 101.0, 101.0, 101.0, ]

class PointCloudTest(unittest.TestCase):
    def __init__(self, *args):
        super(PointCloudTest, self).__init__(*args)
        self.success = False


    def printPointCloud(self, cloud):
        print "["
        i = 0
        for pt in cloud.ranges:
            sys.stdout.write(str(pt) + ", ")
            i = i + 1
            if ((i % 7) == 0):
                print "" #newline
        print "]"

        print "["
        i = 0
        for pt in cloud.intensities:
            sys.stdout.write(str(pt) + ", ")
            i = i + 1
            if ((i % 7) == 0):
                print "" #newline
        print "]"


    def pointInput(self, cloud):
        i = 0
        range_fail_count = 0
        print "Input laser scan received"
        self.printPointCloud(cloud)  #uncomment to capture new data
        while (i < len(cloud.ranges) and i < len(TARGET_RANGES)):
            d = cloud.ranges[i] - TARGET_RANGES[i]
            if ((d < - ERROR_TOL) or (d > ERROR_TOL)):
                range_fail_count += 1
                print "range_fail_count:" + str(range_fail_count) + " failed. error:" + str(d) + " exceeded tolerance:" + str(ERROR_TOL)
            i = i + 1

        i = 0
        intensity_fail_count = 0
        while (i < len(cloud.intensities) and i < len(TARGET_INTENSITIES)):
            d = cloud.intensities[i] - TARGET_INTENSITIES[i]
            if cloud.intensities[i] > 0:
                d = d/cloud.intensities[i]
            if cloud.intensities[i] < 0:
                intensity_fail_count += 1
                print "intensity_fail_count:" + str(intensity_fail_count) + " failed. intensity <0:" + str(cloud.intensiteis[i])
            else:
                if ((d < - ERROR_TOL) or (d > ERROR_TOL)):
                    intensity_fail_count += 1
                    print "intensity_fail_count:" + str(intensity_fail_count) + " failed. error:" + str(d) + " exceeded tolerance:" + str(ERROR_TOL)
            i = i + 1

        if range_fail_count > FAIL_COUNT_TOL:
            print "Range fail count too large (" + str(range_fail_count) + "), failing scan"
            return

        if intensity_fail_count > FAIL_COUNT_TOL:
            print "Intensity fail count too large (" + str(intensity_fail_count) + "), failing scan"
            return

        self.success = True
    
    def test_scan(self):
        print "LNK\n"
        rospy.Subscriber("/base_scan", LaserScan, self.pointInput)
        rospy.init_node(NAME, anonymous=True)
        timeout_t = time.time() + TEST_DURATION
        while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
            time.sleep(0.1)
        self.assert_(self.success)
        
    


if __name__ == '__main__':
    rostest.run(PKG, sys.argv[0], PointCloudTest, sys.argv) #, text_mode=True)