00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00061 void expect_ranges_eq(const std::vector<float> &a, const std::vector<float> &b) {
00062 for( int i=0; i<10; i++) {
00063 if(std::isnan(a[i])) {
00064 EXPECT_TRUE(std::isnan(b[i]));
00065 }
00066 else {
00067 EXPECT_NEAR(a[i], b[i], 1e-6);
00068 }
00069 }
00070 }
00071
00072 TEST(ScanToScanFilterChain, BadConfiguration)
00073 {
00074 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
00075
00076 try
00077 {
00078 filter_chain_.configure("bad_filter_chain");
00079 }
00080 catch(pluginlib::LibraryLoadException)
00081 {
00082 EXPECT_FALSE(false);
00083 }
00084
00085 filter_chain_.clear();
00086 }
00087
00088 TEST(ScanToScanFilterChain, IntensityFilter)
00089 {
00090 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
00091 float nanval = std::numeric_limits<float>::quiet_NaN();
00092 float temp[] = {1.0, nanval, 1.0, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, 2.3};
00093 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
00094 expected_msg.ranges = v1;
00095 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
00096
00097 EXPECT_TRUE(filter_chain_.configure("intensity_filter_chain"));
00098
00099 msg_in = gen_msg();
00100
00101 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00102 expect_ranges_eq(msg_out.ranges, expected_msg.ranges);
00103
00104 filter_chain_.clear();
00105 }
00106
00107 TEST(ScanToScanFilterChain, InterpFilter)
00108 {
00109 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
00110 float temp[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
00111 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
00112 expected_msg.ranges = v1;
00113 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
00114
00115 EXPECT_TRUE(filter_chain_.configure("interp_filter_chain"));
00116
00117 msg_in = gen_msg();
00118
00119 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00120
00121 for( int i=0; i<10; i++){
00122 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-6);
00123 }
00124
00125 filter_chain_.clear();
00126 }
00127
00128 TEST(ScanToScanFilterChain, ShadowFilter)
00129 {
00130 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
00131 float nanval = std::numeric_limits<float>::quiet_NaN();
00132 float temp[] = {nanval, 0.1, nanval, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, nanval};
00133 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
00134 expected_msg.ranges = v1;
00135 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
00136
00137 EXPECT_TRUE(filter_chain_.configure("shadow_filter_chain"));
00138
00139 msg_in = gen_msg();
00140
00141 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00142
00143 expect_ranges_eq(msg_out.ranges, expected_msg.ranges);
00144
00145 filter_chain_.clear();
00146 }
00147
00148 TEST(ScanToScanFilterChain, ArrayFilter)
00149 {
00150 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
00151 float temp[] = {1.0, 0.4, 1.0, 1.0, 1.0, 6.3333, 1.0, 1.0, 1.0, 1.8667};
00152 std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
00153 expected_msg.ranges = v1;
00154 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
00155
00156 EXPECT_TRUE(filter_chain_.configure("array_filter_chain"));
00157
00158 msg_in = gen_msg();
00159
00160 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00161 float temp2[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
00162 std::vector<float> v2 (temp2, temp2 + sizeof(temp2) / sizeof(float));
00163 msg_in.ranges = v2;
00164 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00165 msg_in = gen_msg();
00166 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00167
00168 for( int i=0; i<10; i++){
00169 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-3);
00170 EXPECT_NEAR(msg_out.intensities[i],msg_in.intensities[i],1e-3);
00171 }
00172
00173 filter_chain_.clear();
00174 }
00175
00176 TEST(ScanToScanFilterChain, MaskFilter)
00177 {
00178 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
00179 const float nanval = std::numeric_limits<float>::quiet_NaN();
00180 const float temp[] = {1.0, nanval, 1.0, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, 2.3};
00181 const std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
00182 expected_msg.ranges = v1;
00183 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
00184
00185 EXPECT_TRUE(filter_chain_.configure("mask_filter_chain"));
00186
00187 msg_in = gen_msg();
00188
00189 EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
00190
00191 expect_ranges_eq(msg_out.ranges, expected_msg.ranges);
00192
00193 filter_chain_.clear();
00194 }
00195
00196
00197 int main(int argc, char **argv){
00198 testing::InitGoogleTest(&argc, argv);
00199 ros::init(argc, argv, "test_scan_to_scan_filter_chain");
00200 return RUN_ALL_TESTS();
00201 }