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
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