$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <gtest/gtest.h> 00031 #include <filters/filter_chain.h> 00032 #include <ros/ros.h> 00033 #include "sensor_msgs/LaserScan.h" 00034 #include <pluginlib/class_loader.h> 00035 00036 00037 sensor_msgs::LaserScan gen_msg(){ 00038 sensor_msgs::LaserScan msg; 00039 00040 float temp[] = {1.0, 0.1, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 2.3}; 00041 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float)); 00042 00043 msg.header.stamp = ros::Time::now(); 00044 msg.header.frame_id = "laser"; 00045 msg.angle_min = -.5; 00046 msg.angle_max = .5; 00047 msg.angle_increment = 0.1; 00048 msg.time_increment = 0.1; 00049 msg.scan_time = 0.1; 00050 msg.range_min = 0.5; 00051 msg.range_max = 1.5; 00052 msg.ranges = v1; 00053 msg.intensities = v1; 00054 00055 return msg; 00056 } 00057 00058 TEST(ScanToScanFilterChain, BadConfiguration) 00059 { 00060 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan"); 00061 00062 try 00063 { 00064 filter_chain_.configure("bad_filter_chain"); 00065 } 00066 catch(pluginlib::LibraryLoadException) 00067 { 00068 EXPECT_FALSE(false); 00069 } 00070 00071 filter_chain_.clear(); 00072 } 00073 00074 TEST(ScanToScanFilterChain, IntensityFilter) 00075 { 00076 sensor_msgs::LaserScan msg_in, msg_out, expected_msg; 00077 float temp[] = {1.0, 2.5, 1.0, 1.0, 1.0, 2.5, 1.0, 1.0, 1.0, 2.3}; 00078 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float)); 00079 expected_msg.ranges = v1; 00080 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan"); 00081 00082 EXPECT_TRUE(filter_chain_.configure("intensity_filter_chain")); 00083 00084 msg_in = gen_msg(); 00085 00086 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out)); 00087 00088 for( int i=0; i<10; i++){ 00089 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-6); 00090 } 00091 00092 filter_chain_.clear(); 00093 } 00094 00095 TEST(ScanToScanFilterChain, InterpFilter) 00096 { 00097 sensor_msgs::LaserScan msg_in, msg_out, expected_msg; 00098 float temp[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; 00099 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float)); 00100 expected_msg.ranges = v1; 00101 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan"); 00102 00103 EXPECT_TRUE(filter_chain_.configure("interp_filter_chain")); 00104 00105 msg_in = gen_msg(); 00106 00107 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out)); 00108 00109 for( int i=0; i<10; i++){ 00110 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-6); 00111 } 00112 00113 filter_chain_.clear(); 00114 } 00115 00116 TEST(ScanToScanFilterChain, ShadowFilter) 00117 { 00118 sensor_msgs::LaserScan msg_in, msg_out, expected_msg; 00119 float temp[] = {-1.0, 0.1, -1.0, 1.0, 1.0, -9.0, 1.0, 1.0, 1.0, -2.3}; 00120 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float)); 00121 expected_msg.ranges = v1; 00122 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan"); 00123 00124 EXPECT_TRUE(filter_chain_.configure("shadow_filter_chain")); 00125 00126 msg_in = gen_msg(); 00127 00128 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out)); 00129 00130 for( int i=0; i<10; i++){ 00131 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-6); 00132 } 00133 00134 filter_chain_.clear(); 00135 } 00136 00137 TEST(ScanToScanFilterChain, ArrayFilter) 00138 { 00139 sensor_msgs::LaserScan msg_in, msg_out, expected_msg; 00140 float temp[] = {1.0, 0.4, 1.0, 1.0, 1.0, 6.3333, 1.0, 1.0, 1.0, 1.8667}; 00141 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float)); 00142 expected_msg.ranges = v1; 00143 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan"); 00144 00145 EXPECT_TRUE(filter_chain_.configure("array_filter_chain")); 00146 00147 msg_in = gen_msg(); 00148 00149 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out)); 00150 float temp2[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; 00151 std::vector<float> v2 (temp2, temp2 + sizeof(temp2) / sizeof(float)); 00152 msg_in.ranges = v2; 00153 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out)); 00154 msg_in = gen_msg(); 00155 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out)); 00156 00157 for( int i=0; i<10; i++){ 00158 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-3); 00159 EXPECT_NEAR(msg_out.intensities[i],msg_in.intensities[i],1e-3); 00160 } 00161 00162 filter_chain_.clear(); 00163 } 00164 00165 00166 int main(int argc, char **argv){ 00167 testing::InitGoogleTest(&argc, argv); 00168 ros::init(argc, argv, "test_scan_to_scan_filter_chain"); 00169 return RUN_ALL_TESTS(); 00170 } 00171