test_pr2_sensors_gazebo: /opt/ros/diamondback/stacks/wg_robots_gazebo/test_pr2_sensors_gazebo/test

00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 ## Gazebo test cameras validation 
00036 
00037 PKG = 'test_pr2_sensors_gazebo'
00038 NAME = 'test_scan'
00039 
00040 import math
00041 import roslib
00042 roslib.load_manifest(PKG)
00043 
00044 
00045 import sys, unittest
00046 import os, os.path, threading, time
00047 import rospy, rostest
00048 from sensor_msgs.msg import LaserScan
00049 
00050 TEST_DURATION  = 25
00051 ERROR_TOL      = 0.05
00052 FAIL_COUNT_TOL = 10
00053 
00054 TARGET_RANGES = [
00055 10.0, 10.0, 10.0, 9.99573993683, 9.99354457855, 10.0, 9.99495887756, 
00056 9.99537754059, 9.99530220032, 9.99065494537, 9.99572181702, 10.0, 9.99661064148, 10.0, 
00057 9.9931268692, 10.0, 9.99523544312, 9.99910163879, 10.0, 9.99757385254, 10.0, 
00058 10.0, 10.0, 9.98721313477, 10.0, 10.0, 9.99776268005, 9.98858833313, 
00059 9.99840354919, 10.0, 9.9975643158, 10.0, 10.0, 9.9985332489, 9.9998550415, 
00060 9.99693107605, 10.0, 9.99122810364, 10.0, 9.9965057373, 9.99392700195, 9.9958820343, 
00061 9.99862098694, 10.0, 10.0, 10.0, 10.0, 10.0, 9.99697303772, 
00062 10.0, 9.99373340607, 10.0, 9.98993110657, 10.0, 9.99621105194, 10.0, 
00063 10.0, 9.99318027496, 9.99940681458, 9.99647140503, 10.0, 9.99137496948, 10.0, 
00064 9.99066162109, 9.99819469452, 9.99851989746, 9.99762248993, 9.99655723572, 10.0, 9.9974155426, 
00065 9.99393177032, 10.0, 10.0, 10.0, 10.0, 9.99311447144, 10.0, 
00066 10.0, 9.99642086029, 9.99772071838, 9.99639415741, 10.0, 10.0, 9.99461174011, 
00067 10.0, 9.99182224274, 9.99395465851, 10.0, 10.0, 9.99905490875, 10.0, 
00068 9.9945936203, 10.0, 9.99512577057, 9.99258613586, 9.99485301971, 10.0, 9.99732017517, 
00069 9.99589729309, 9.99851226807, 10.0, 10.0, 9.99726200104, 10.0, 10.0, 
00070 10.0, 10.0, 9.99561405182, 9.9951543808, 10.0, 9.99977779388, 9.99529075623, 
00071 10.0, 10.0, 9.99410533905, 9.98627853394, 9.9893321991, 10.0, 10.0, 
00072 9.99817180634, 9.9997959137, 10.0, 9.9965801239, 10.0, 10.0, 9.99580669403, 
00073 10.0, 10.0, 9.99091243744, 9.99701595306, 10.0, 9.9877243042, 9.99554157257, 
00074 9.99643039703, 10.0, 10.0, 10.0, 10.0, 9.99929332733, 9.99739742279, 
00075 9.99473190308, 10.0, 10.0, 10.0, 9.9960603714, 10.0, 9.99765872955, 
00076 9.9982509613, 9.99988269806, 10.0, 10.0, 10.0, 10.0, 9.99551677704, 
00077 9.99934577942, 9.99937820435, 9.99880981445, 10.0, 9.99725151062, 10.0, 10.0, 
00078 10.0, 10.0, 9.99893283844, 9.99085235596, 10.0, 10.0, 9.99376106262, 
00079 9.99191761017, 10.0, 9.99659919739, 9.99622631073, 9.99751281738, 10.0, 9.99464511871, 
00080 9.99992275238, 10.0, 9.98847484589, 10.0, 10.0, 9.99902820587, 10.0, 
00081 9.99963665009, 10.0, 9.99925041199, 9.99624538422, 9.99273204803, 10.0, 9.99636650085, 
00082 10.0, 9.99062824249, 10.0, 9.99874019623, 10.0, 9.99279022217, 10.0, 
00083 9.9953546524, 9.99644756317, 9.98907089233, 10.0, 10.0, 9.99982070923, 10.0, 
00084 9.99953460693, 10.0, 10.0, 9.99766349792, 9.9923210144, 10.0, 10.0, 
00085 10.0, 10.0, 9.99549865723, 9.9989824295, 9.98648643494, 9.99997234344, 10.0, 
00086 10.0, 10.0, 9.99065971375, 9.99425029755, 10.0, 9.99474906921, 10.0, 
00087 10.0, 10.0, 10.0, 9.99846553802, 9.99258041382, 9.99472618103, 9.99771308899, 
00088 10.0, 10.0, 9.99508285522, 9.99805355072, 9.99019050598, 9.9955291748, 2.03685235977, 
00089 2.04421663284, 2.03210663795, 2.0194671154, 2.0155646801, 1.99427628517, 2.00035619736, 1.98695075512, 
00090 1.97852289677, 1.96805429459, 1.9588958025, 1.95648872852, 1.94692718983, 1.93678343296, 1.91842794418, 
00091 1.92827701569, 1.91704964638, 1.91358006001, 1.92673146725, 1.94037222862, 9.99822807312, 10.0, 
00092 10.0, 10.0, 2.14445352554, 2.08714962006, 2.05095148087, 2.02395749092, 2.00428080559, 
00093 1.97976839542, 1.95980763435, 1.9413497448, 1.93820095062, 1.91665804386, 1.92057430744, 1.9003251791, 
00094 1.88783442974, 1.8934186697, 1.8741286993, 1.86705768108, 1.85493171215, 1.85695803165, 1.85333991051, 
00095 1.83796668053, 1.84245407581, 1.84312713146, 1.84570860863, 1.82822322845, 1.83273553848, 1.83729255199, 
00096 1.83289527893, 1.8334633112, 1.83232152462, 1.82959234715, 1.8262963295, 1.83373129368, 1.83350098133, 
00097 1.83656823635, 1.83848130703, 1.83709740639, 1.85071253777, 1.84394586086, 1.86468243599, 1.85489320755, 
00098 1.85955429077, 1.86517083645, 1.88544261456, 1.89780557156, 1.89888262749, 1.92238008976, 1.922524333, 
00099 1.93953430653, 1.948564291, 1.96180999279, 1.98394215107, 1.99410378933, 2.02381777763, 2.05232095718, 
00100 2.09428310394, 2.14106678963, 2.51629209518, 2.51145839691, 2.50856781006, 2.49926424026, 2.51138210297, 
00101 2.50430202484, 2.50846076012, 2.52427816391, 2.52740097046, 2.5371067524, 2.54831123352, 2.56848597527, 
00102 2.59808444977, 2.62111496925, 2.68968343735, 10.0, 9.99606513977, 10.0, 10.0, 
00103 10.0, 10.0, 9.99753856659, 9.99373626709, 10.0, 2.72182154655, 2.7262597084, 
00104 10.0, 9.99517822266, 9.99814987183, 9.99634075165, 10.0, 10.0, 10.0, 
00105 10.0, 9.99925136566, 10.0, 10.0, 10.0, 10.0, 9.99726676941, 
00106 10.0, 10.0, 1.80504393578, 1.76711213589, 1.73867917061, 1.7188462019, 1.706569314, 
00107 1.69170439243, 1.67706441879, 1.66418457031, 1.6565977335, 1.64732301235, 1.64360272884, 1.63184809685, 
00108 1.63481676579, 1.62855827808, 1.62778234482, 1.61982572079, 1.6105659008, 1.6124740839, 1.60954415798, 
00109 1.6100025177, 1.60651493073, 1.59700119495, 1.60250425339, 1.60432767868, 1.602414608, 1.60677278042, 
00110 1.61422848701, 1.61278867722, 1.61149096489, 1.61712217331, 1.61782002449, 1.63084983826, 1.63016283512, 
00111 1.64222669601, 1.64433014393, 1.65826809406, 1.66858124733, 1.67725527287, 1.68793737888, 1.70181775093, 
00112 1.71679854393, 1.74195897579, 1.76237237453, 1.80084371567, 10.0, 9.98779392242, 9.99672698975, 
00113 10.0, 9.99886322021, 9.99884319305, 10.0, 10.0, 10.0, 10.0, 
00114 9.99102210999, 10.0, 10.0, 9.9920835495, 9.99745178223, 10.0, 9.99134922028, 
00115 10.0, 9.9976682663, 9.99491119385, 9.99395751953, 9.9923286438, 9.99616718292, 9.99658679962, 
00116 9.98934459686, 10.0, 9.99688911438, 9.99960327148, 10.0, 9.99228954315, 10.0, 
00117 9.99639987946, 9.99746799469, 9.99986839294, 9.99951839447, 9.99859428406, 10.0, 10.0, 
00118 10.0, 9.99822998047, 9.99964332581, 9.99930763245, 9.99730014801, 9.99543762207, 9.99613285065, 
00119 10.0, 9.99729442596, 10.0, 10.0, 10.0, 10.0, 9.99937438965, 
00120 9.99928283691, 9.99720096588, 9.99948310852, 9.9946680069, 9.99388885498, 10.0, 10.0, 
00121 10.0, 10.0, 9.99677371979, 9.99859809875, 10.0, 10.0, 9.99358463287, 
00122 9.99317646027, 10.0, 9.99491786957, 10.0, 10.0, 9.99915504456, 10.0, 
00123 9.99907207489, 10.0, 9.99767684937, 10.0, 10.0, 9.99805450439, 10.0, 
00124 9.98858165741, 9.99410438538, 10.0, 9.99862289429, 10.0, 10.0, 10.0, 
00125 9.99983596802, 9.99750804901, 10.0, 9.99136257172, 9.99489784241, 10.0, 9.99796485901, 
00126 10.0, 10.0, 10.0, 9.99649906158, 9.99518966675, 10.0, 10.0, 
00127 9.9939289093, 9.9948425293, 9.99791431427, 9.99170398712, 10.0, 9.99482250214, 9.99916267395, 
00128 9.99699401855, 10.0, 9.99531364441, 10.0, 9.99355602264, 10.0, 9.99841976166, 
00129 10.0, 10.0, 9.99886226654, 9.99558353424, 10.0, 10.0, 9.99983596802, 
00130 10.0, 9.99355125427, 9.99629402161, 9.99861049652, 9.99837779999, 9.99851799011, 10.0, 
00131 10.0, 9.99499320984, 9.99850845337, 9.99886894226, 10.0, 9.99392700195, 9.99863529205, 
00132 9.99880886078, 9.99745464325, 9.99819660187, 9.99894714355, 10.0, 9.99813079834, 10.0, 
00133 10.0, 9.99911022186, 10.0, 9.99262619019, 9.99907207489, 9.994389534, 10.0, 
00134 9.99777507782, 9.99799919128, 9.99109649658, 9.99544143677, 9.99902153015, 10.0, 9.9965801239, 
00135 10.0, 9.99814796448, 9.99841499329, 10.0, 9.9998960495, 10.0, 10.0, 
00136 9.99292850494, 10.0, 9.99964237213, 10.0, 10.0, 9.99912929535, 9.99788856506, 
00137 9.99653816223, 10.0, 10.0, 9.99705600739, 10.0, 10.0, 10.0, 
00138 10.0, 9.99820232391, 10.0, 9.99996948242, 9.99960136414, 10.0, 10.0, 
00139 10.0, 9.99853801727, 10.0, 10.0, 10.0, 9.99934387207, 10.0, 
00140 10.0, 9.9871711731, 9.99130916595, 10.0, 10.0, 10.0, 9.99577236176, 
00141 10.0, 10.0, 9.99879455566, 10.0, 10.0, 9.99161338806, 9.99415493011, 
00142 10.0, 10.0, 10.0, 9.99282264709, 9.99699115753, 9.99679279327, 10.0, 
00143 9.99227809906, 9.99905967712, 9.9926738739, 10.0, 10.0, 10.0, 9.99697971344, 
00144 9.99261283875, 9.98644828796, 9.99855899811, 10.0, 9.99784469604, 10.0, 10.0, 
00145 10.0, 9.99215602875, 9.98868179321, 9.99840354919, 9.99383068085, 10.0, 9.99576854706, 
00146 9.99744510651, 10.0, 10.0, ]
00147 
00148 
00149 
00150 
00151 
00152 
00153 TARGET_INTENSITIES = [
00154 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00155 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00156 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00157 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00158 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00159 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00160 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00161 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00162 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00163 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00164 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00165 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00166 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00167 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00168 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00169 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00170 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00171 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00172 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00173 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00174 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00175 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00176 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00177 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00178 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00179 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00180 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00181 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00182 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00183 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00184 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00185 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00186 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00187 101.0, 101.0, 101.0, 101.0, 101.0, 250.002059937, 500.003509521, 
00188 500.005279541, 499.994689941, 500.009216309, 499.99597168, 499.988525391, 499.999633789, 499.992553711, 
00189 499.995391846, 499.997558594, 500.000244141, 500.001342773, 499.990570068, 499.999420166, 499.998443604, 
00190 500.003082275, 500.000579834, 500.011169434, 499.998931885, 249.997711182, 101.0, 101.0, 
00191 101.0, 1500.00146484, 3000.0, 3000.00292969, 3000.00268555, 3000.00878906, 3000.00415039, 
00192 3000.00097656, 2999.99853516, 3000.00805664, 3000.00146484, 3000.00415039, 2999.99633789, 2999.99609375, 
00193 3000.00561523, 2999.99438477, 2999.99584961, 2999.99951172, 2999.99536133, 3000.00341797, 3000.00146484, 
00194 2999.99536133, 2999.99633789, 3000.00048828, 3000.00268555, 2999.99243164, 3000.00415039, 2999.99853516, 
00195 3000.00439453, 3000.00708008, 2999.99316406, 3000.00366211, 3000.0, 3000.0065918, 2999.99780273, 
00196 3000.0, 2999.99121094, 2999.99902344, 3000.00195312, 2999.98901367, 2999.9934082, 2999.99951172, 
00197 2999.99584961, 3000.00366211, 2999.99829102, 2999.99902344, 3000.00292969, 2999.99853516, 2999.99902344, 
00198 2999.99658203, 2999.99829102, 2999.99658203, 3000.00146484, 3000.00195312, 3000.00878906, 2999.99414062, 
00199 3000.00097656, 2499.99438477, 2000.00012207, 2000.00183105, 1999.98620605, 1999.99951172, 1999.99206543, 
00200 2000.00280762, 1999.99804688, 2000.00195312, 1999.99401855, 1999.99853516, 1999.9901123, 1999.99731445, 
00201 2000.00256348, 2000.00927734, 1000.00360107, 101.0, 101.0, 101.0, 101.0, 
00202 101.0, 101.0, 101.0, 101.0, 1000.0012207, 1999.9967041, 1000.00085449, 
00203 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00204 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00205 101.0, 500.003265381, 999.998596191, 999.997680664, 999.991210938, 999.996032715, 1000.00134277, 
00206 1000.00958252, 999.99786377, 1000.0055542, 999.996276855, 1000.00384521, 999.996765137, 1000.00292969, 
00207 1000.00128174, 999.996154785, 1000.0032959, 1000.00061035, 999.99609375, 999.99621582, 999.996520996, 
00208 1000.00500488, 999.997253418, 999.983886719, 1000.00042725, 999.998596191, 999.996887207, 999.992370605, 
00209 1000.00213623, 1000.00799561, 999.997680664, 1000.00170898, 1000.00073242, 999.996398926, 1000.00830078, 
00210 1000.00238037, 999.993774414, 999.993713379, 999.989990234, 999.996765137, 1000.00146484, 1000.00634766, 
00211 999.990966797, 1000.00305176, 999.996704102, 499.996307373, 101.0, 101.0, 101.0, 
00212 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00213 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00214 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, 
00215 101.0, 101.0, 101.0, 101.0, 101.0, 101.0, ]
00216 
00217 class PointCloudTest(unittest.TestCase):
00218     def __init__(self, *args):
00219         super(PointCloudTest, self).__init__(*args)
00220         self.success = False
00221 
00222 
00223     def printPointCloud(self, cloud):
00224         print "["
00225         i = 0
00226         for pt in cloud.ranges:
00227             sys.stdout.write(str(pt) + ", ")
00228             i = i + 1
00229             if ((i % 7) == 0):
00230                 print "" #newline
00231         print "]"
00232 
00233         print "["
00234         i = 0
00235         for pt in cloud.intensities:
00236             sys.stdout.write(str(pt) + ", ")
00237             i = i + 1
00238             if ((i % 7) == 0):
00239                 print "" #newline
00240         print "]"
00241 
00242 
00243     def pointInput(self, cloud):
00244         i = 0
00245         range_fail_count = 0
00246         print "Input laser scan received"
00247         self.printPointCloud(cloud)  #uncomment to capture new data
00248         while (i < len(cloud.ranges) and i < len(TARGET_RANGES)):
00249             d = cloud.ranges[i] - TARGET_RANGES[i]
00250             if ((d < - ERROR_TOL) or (d > ERROR_TOL)):
00251                 range_fail_count += 1
00252                 print "range_fail_count:" + str(range_fail_count) + " failed. error:" + str(d) + " exceeded tolerance:" + str(ERROR_TOL)
00253             i = i + 1
00254 
00255         i = 0
00256         intensity_fail_count = 0
00257         while (i < len(cloud.intensities) and i < len(TARGET_INTENSITIES)):
00258             d = cloud.intensities[i] - TARGET_INTENSITIES[i]
00259             if cloud.intensities[i] > 0:
00260                 d = d/cloud.intensities[i]
00261             if cloud.intensities[i] < 0:
00262                 intensity_fail_count += 1
00263                 print "intensity_fail_count:" + str(intensity_fail_count) + " failed. intensity <0:" + str(cloud.intensiteis[i])
00264             else:
00265                 if ((d < - ERROR_TOL) or (d > ERROR_TOL)):
00266                     intensity_fail_count += 1
00267                     print "intensity_fail_count:" + str(intensity_fail_count) + " failed. error:" + str(d) + " exceeded tolerance:" + str(ERROR_TOL)
00268             i = i + 1
00269 
00270         if range_fail_count > FAIL_COUNT_TOL:
00271             print "Range fail count too large (" + str(range_fail_count) + "), failing scan"
00272             return
00273 
00274         if intensity_fail_count > FAIL_COUNT_TOL:
00275             print "Intensity fail count too large (" + str(intensity_fail_count) + "), failing scan"
00276             return
00277 
00278         self.success = True
00279     
00280     def test_scan(self):
00281         print "LNK\n"
00282         rospy.Subscriber("/base_scan", LaserScan, self.pointInput)
00283         rospy.init_node(NAME, anonymous=True)
00284         timeout_t = time.time() + TEST_DURATION
00285         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
00286             time.sleep(0.1)
00287         self.assert_(self.success)
00288         
00289     
00290 
00291 
00292 if __name__ == '__main__':
00293     rostest.run(PKG, sys.argv[0], PointCloudTest, sys.argv) #, text_mode=True)
00294 
00295
/opt/ros/diamondback/stacks/wg_robots_gazebo/test_pr2_sensors_gazebo/test_scan.py
