30 #include <gtest/gtest.h> 33 #include "sensor_msgs/LaserScan.h" 38 sensor_msgs::LaserScan msg;
40 float temp[] = {1.0, 0.1, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 2.3};
41 std::vector<float> v1 (temp, temp +
sizeof(temp) /
sizeof(
float));
44 msg.header.frame_id =
"laser";
47 msg.angle_increment = 0.1;
48 msg.time_increment = 0.1;
62 for(
int i=0; i<10; i++) {
63 if(std::isnan(a[i])) {
64 EXPECT_TRUE(std::isnan(b[i]));
67 EXPECT_NEAR(a[i], b[i], 1e-6);
78 filter_chain_.
configure(
"bad_filter_chain");
85 filter_chain_.
clear();
90 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
91 float nanval = std::numeric_limits<float>::quiet_NaN();
92 float temp[] = {1.0, nanval, 1.0, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, 2.3};
93 std::vector<float> v1 (temp, temp +
sizeof(temp) /
sizeof(
float));
94 expected_msg.ranges = v1;
97 EXPECT_TRUE(filter_chain_.
configure(
"intensity_filter_chain"));
101 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
104 filter_chain_.
clear();
109 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
110 float temp[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
111 std::vector<float> v1 (temp, temp +
sizeof(temp) /
sizeof(
float));
112 expected_msg.ranges = v1;
115 EXPECT_TRUE(filter_chain_.
configure(
"interp_filter_chain"));
119 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
121 for(
int i=0; i<10; i++){
122 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-6);
125 filter_chain_.
clear();
130 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
131 float nanval = std::numeric_limits<float>::quiet_NaN();
132 float temp[] = {nanval, 0.1, nanval, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, nanval};
133 std::vector<float> v1 (temp, temp +
sizeof(temp) /
sizeof(
float));
134 expected_msg.ranges = v1;
137 EXPECT_TRUE(filter_chain_.
configure(
"shadow_filter_chain"));
141 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
145 filter_chain_.
clear();
150 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
151 float temp[] = {1.0, 0.4, 1.0, 1.0, 1.0, 6.3333, 1.0, 1.0, 1.0, 1.8667};
152 std::vector<float> v1 (temp, temp +
sizeof(temp) /
sizeof(
float));
153 expected_msg.ranges = v1;
156 EXPECT_TRUE(filter_chain_.
configure(
"array_filter_chain"));
160 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
161 float temp2[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
162 std::vector<float> v2 (temp2, temp2 +
sizeof(temp2) /
sizeof(
float));
164 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
166 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
168 for(
int i=0; i<10; i++){
169 EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-3);
170 EXPECT_NEAR(msg_out.intensities[i],msg_in.intensities[i],1e-3);
173 filter_chain_.
clear();
178 sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
179 const float nanval = std::numeric_limits<float>::quiet_NaN();
180 const float temp[] = {1.0, nanval, 1.0, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, 2.3};
181 const std::vector<float> v1 (temp, temp +
sizeof(temp) /
sizeof(
float));
182 expected_msg.ranges = v1;
185 EXPECT_TRUE(filter_chain_.
configure(
"mask_filter_chain"));
189 EXPECT_TRUE(filter_chain_.
update(msg_in, msg_out));
193 filter_chain_.
clear();
197 int main(
int argc,
char **argv){
198 testing::InitGoogleTest(&argc, argv);
199 ros::init(argc, argv,
"test_scan_to_scan_filter_chain");
200 return RUN_ALL_TESTS();
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void expect_ranges_eq(const std::vector< float > &a, const std::vector< float > &b)
bool update(const T &data_in, T &data_out)
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
sensor_msgs::LaserScan gen_msg()
TEST(ScanToScanFilterChain, BadConfiguration)