@@ -145,78 +145,6 @@ TEST_F(FilterTest, TestExponentialWrenchFilterComputation)
145145 ASSERT_NEAR (out.wrench .torque .z , calculated.wrench .torque .z , 1e-9 );
146146}
147147
148- TEST_F (FilterTest, TestExponentialWrenchFilterAllParameters)
149- {
150- double alpha = 0.7 ;
151- node_->declare_parameter (" alpha" , rclcpp::ParameterValue (alpha));
152- geometry_msgs::msg::WrenchStamped in, calculated, out;
153- in.header .frame_id = " world" ;
154- in.wrench .force .x = 1.0 ;
155- in.wrench .force .y = 2.0 ;
156- in.wrench .force .z = 3.0 ;
157- in.wrench .torque .x = 10.0 ;
158- in.wrench .torque .y = 20.0 ;
159- in.wrench .torque .z = 30.0 ;
160-
161- std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
162- std::make_shared<control_filters::ExponentialFilter<geometry_msgs::msg::WrenchStamped>>();
163-
164- ASSERT_TRUE (filter_->configure (
165- " " , " TestExponentialFilter" , node_->get_node_logging_interface (),
166- node_->get_node_parameters_interface ()));
167-
168- // First filter pass, output should be equal to the input
169- ASSERT_TRUE (filter_->update (in, out));
170- ASSERT_EQ (out.wrench .force .x , in.wrench .force .x );
171- ASSERT_EQ (out.wrench .force .y , in.wrench .force .y );
172- ASSERT_EQ (out.wrench .force .z , in.wrench .force .z );
173- ASSERT_EQ (out.wrench .torque .x , in.wrench .torque .x );
174- ASSERT_EQ (out.wrench .torque .y , in.wrench .torque .y );
175- ASSERT_EQ (out.wrench .torque .z , in.wrench .torque .z );
176-
177- // Initialize calculated with first output
178- calculated = out;
179-
180- // Second filter pass with a step in values
181- in.wrench .force .x = 2.0 ;
182- in.wrench .force .y = 4.0 ;
183- in.wrench .force .z = 6.0 ;
184- in.wrench .torque .x = 20.0 ;
185- in.wrench .torque .y = 40.0 ;
186- in.wrench .torque .z = 60.0 ;
187- ASSERT_TRUE (filter_->update (in, out));
188-
189- // Manual exponential smoothing calculation for all fields
190- calculated.wrench .force .x = alpha * in.wrench .force .x + (1.0 - alpha) * calculated.wrench .force .x ;
191- calculated.wrench .force .y = alpha * in.wrench .force .y + (1.0 - alpha) * calculated.wrench .force .y ;
192- calculated.wrench .force .z = alpha * in.wrench .force .z + (1.0 - alpha) * calculated.wrench .force .z ;
193- calculated.wrench .torque .x =
194- alpha * in.wrench .torque .x + (1.0 - alpha) * calculated.wrench .torque .x ;
195- calculated.wrench .torque .y =
196- alpha * in.wrench .torque .y + (1.0 - alpha) * calculated.wrench .torque .y ;
197- calculated.wrench .torque .z =
198- alpha * in.wrench .torque .z + (1.0 - alpha) * calculated.wrench .torque .z ;
199-
200- // Third filter pass, check against manual calculation
201- ASSERT_TRUE (filter_->update (in, out));
202- calculated.wrench .force .x = alpha * in.wrench .force .x + (1.0 - alpha) * calculated.wrench .force .x ;
203- calculated.wrench .force .y = alpha * in.wrench .force .y + (1.0 - alpha) * calculated.wrench .force .y ;
204- calculated.wrench .force .z = alpha * in.wrench .force .z + (1.0 - alpha) * calculated.wrench .force .z ;
205- calculated.wrench .torque .x =
206- alpha * in.wrench .torque .x + (1.0 - alpha) * calculated.wrench .torque .x ;
207- calculated.wrench .torque .y =
208- alpha * in.wrench .torque .y + (1.0 - alpha) * calculated.wrench .torque .y ;
209- calculated.wrench .torque .z =
210- alpha * in.wrench .torque .z + (1.0 - alpha) * calculated.wrench .torque .z ;
211-
212- ASSERT_NEAR (out.wrench .force .x , calculated.wrench .force .x , 1e-9 );
213- ASSERT_NEAR (out.wrench .force .y , calculated.wrench .force .y , 1e-9 );
214- ASSERT_NEAR (out.wrench .force .z , calculated.wrench .force .z , 1e-9 );
215- ASSERT_NEAR (out.wrench .torque .x , calculated.wrench .torque .x , 1e-9 );
216- ASSERT_NEAR (out.wrench .torque .y , calculated.wrench .torque .y , 1e-9 );
217- ASSERT_NEAR (out.wrench .torque .z , calculated.wrench .torque .z , 1e-9 );
218- }
219-
220148TEST_F (FilterTest, TestExponentialWrenchFilterMissingParameter)
221149{
222150 std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
0 commit comments