Skip to content

Commit ec19b21

Browse files
authored
Update plugin lib exception handling (#263)
1 parent e7049ab commit ec19b21

File tree

5 files changed

+31
-17
lines changed

5 files changed

+31
-17
lines changed

include/control_filters/exponential_filter.hpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -86,13 +86,14 @@ bool ExponentialFilter<T>::configure()
8686
std::make_shared<exponential_filter::ParamListener>(this->params_interface_,
8787
this->param_prefix_);
8888
}
89-
catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
90-
RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what());
89+
catch (const std::exception & ex) {
90+
RCLCPP_ERROR((*logger_),
91+
"Exponential filter cannot be configured: %s (type : %s)", ex.what(), typeid(ex).name());
9192
parameter_handler_.reset();
9293
return false;
9394
}
94-
catch (rclcpp::exceptions::InvalidParameterValueException & ex) {
95-
RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what());
95+
catch (...) {
96+
RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring Exponential filter");
9697
parameter_handler_.reset();
9798
return false;
9899
}

include/control_filters/low_pass_filter.hpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -112,13 +112,14 @@ bool LowPassFilter<T>::configure()
112112
std::make_shared<low_pass_filter::ParamListener>(this->params_interface_,
113113
this->param_prefix_);
114114
}
115-
catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
116-
RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
115+
catch (const std::exception & ex) {
116+
RCLCPP_ERROR((*logger_),
117+
"LowPass filter cannot be configured: %s (type : %s)", ex.what(), typeid(ex).name());
117118
parameter_handler_.reset();
118119
return false;
119120
}
120-
catch (rclcpp::exceptions::InvalidParameterValueException & ex) {
121-
RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
121+
catch (...) {
122+
RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring LowPass filter");
122123
parameter_handler_.reset();
123124
return false;
124125
}

include/control_filters/rate_limiter.hpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -89,13 +89,14 @@ bool RateLimiter<T>::configure()
8989
std::make_shared<rate_limiter::ParamListener>(this->params_interface_,
9090
this->param_prefix_);
9191
}
92-
catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
93-
RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what());
92+
catch (const std::exception & ex) {
93+
RCLCPP_ERROR((*logger_),
94+
"Rate Limiter filter cannot be configured: %s (type : %s)", ex.what(), typeid(ex).name());
9495
parameter_handler_.reset();
9596
return false;
9697
}
97-
catch (rclcpp::exceptions::InvalidParameterValueException & ex) {
98-
RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what());
98+
catch (...) {
99+
RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring Rate Limiter filter");
99100
parameter_handler_.reset();
100101
return false;
101102
}

test/control_filters/test_exponential_filter.cpp

+12-5
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,13 @@ TEST_F(FilterTest, TestExponentialFilterThrowsUnconfigured)
2727
ASSERT_THROW(filter_->update(in, out), std::runtime_error);
2828
}
2929

30+
TEST_F(FilterTest, TestExponentialFilterInvalidParameterValue)
31+
{
32+
std::shared_ptr<filters::FilterBase<double>> filter_ =
33+
std::make_shared<control_filters::ExponentialFilter<double>>();
34+
ASSERT_FALSE(filter_->configure("", "TestExponentialFilter",
35+
node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
36+
}
3037

3138
TEST_F(FilterTest, TestExponentialFilterComputation)
3239
{
@@ -47,13 +54,13 @@ TEST_F(FilterTest, TestExponentialFilterComputation)
4754
ASSERT_EQ(out, 1.0);
4855

4956
// second filter pass with same values: no change
50-
// check equality with low-pass-filter
51-
ASSERT_TRUE(filter_->update(in, out));
52-
calculated = in;
53-
ASSERT_EQ(calculated, out);
57+
// check equality with low-pass-filter
58+
ASSERT_TRUE(filter_->update(in, out));
59+
calculated = in;
60+
ASSERT_EQ(calculated, out);
5461

5562
// input change
56-
in = 0.0;
63+
in = 0.0;
5764
for (int i = 0; i < 100; ++i){
5865
ASSERT_TRUE(filter_->update(in, out));
5966
calculated = alpha * in + (1 - alpha) * calculated;
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,7 @@
11
TestExponentialFilterComputation:
22
ros__parameters:
33
alpha: 0.7
4+
5+
TestExponentialInvalidParameterValue:
6+
ros__parameters:
7+
alpha: "a"

0 commit comments

Comments
 (0)