Merge branch 'ros2-master' into pid_double

Author: christophfroehlich

CMakeLists.txt

@@ -115,7 +115,6 @@ add_library(exponential_filter SHARED
 target_compile_features(exponential_filter PUBLIC cxx_std_17)
 target_include_directories(exponential_filter PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
     $<INSTALL_INTERFACE:include/control_toolbox>
 )
 target_link_libraries(exponential_filter PUBLIC
@@ -148,21 +147,37 @@ if(BUILD_TESTING)
   ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle)
 
   ## Control Filters
+  # exponential_filter
+  add_rostest_with_parameters_gmock(test_exponential_filter test/control_filters/test_exponential_filter.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_exponential_filter_parameters.yaml
+  )
+  target_link_libraries(test_exponential_filter exponential_filter exponential_filter_parameters)
+  ament_target_dependencies(test_exponential_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+  set_tests_properties(test_exponential_filter PROPERTIES TIMEOUT 2)
+
+  ament_add_gmock(test_load_exponential_filter test/control_filters/test_load_exponential_filter.cpp)
+  target_link_libraries(test_load_exponential_filter exponential_filter exponential_filter_parameters)
+  ament_target_dependencies(test_load_exponential_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
+  # low_pass_filter
   add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml
   )
   target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters)
   ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+  set_tests_properties(test_low_pass_filter PROPERTIES TIMEOUT 2)
 
   ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp)
   target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters)
   ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 
+  # rate_limiter
   add_rostest_with_parameters_gmock(test_rate_limiter test/control_filters/test_rate_limiter.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_rate_limiter_parameters.yaml
   )
   target_link_libraries(test_rate_limiter rate_limiter rate_limiter_parameters)
   ament_target_dependencies(test_rate_limiter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+  set_tests_properties(test_rate_limiter PROPERTIES TIMEOUT 2)
 
   ament_add_gmock(test_load_rate_limiter test/control_filters/test_load_rate_limiter.cpp)
   target_link_libraries(test_load_rate_limiter rate_limiter rate_limiter_parameters)
@@ -177,6 +192,7 @@ install(
 install(TARGETS control_toolbox
   low_pass_filter low_pass_filter_parameters
   rate_limiter rate_limiter_parameters
+  exponential_filter exponential_filter_parameters
   EXPORT export_control_toolbox
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

include/control_filters/exponential_filter.hpp

@@ -86,13 +86,14 @@ bool ExponentialFilter<T>::configure()
         std::make_shared<exponential_filter::ParamListener>(this->params_interface_,
                                                          this->param_prefix_);
     }
-    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
-      RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what());
+    catch (const std::exception & ex) {
+      RCLCPP_ERROR((*logger_),
+        "Exponential filter cannot be configured: %s (type : %s)", ex.what(), typeid(ex).name());
       parameter_handler_.reset();
       return false;
     }
-    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
-      RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what());
+    catch (...) {
+      RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring Exponential filter");
       parameter_handler_.reset();
       return false;
     }

include/control_filters/low_pass_filter.hpp

@@ -112,13 +112,14 @@ bool LowPassFilter<T>::configure()
         std::make_shared<low_pass_filter::ParamListener>(this->params_interface_,
                                                          this->param_prefix_);
     }
-    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
-      RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
+    catch (const std::exception & ex) {
+      RCLCPP_ERROR((*logger_),
+        "LowPass filter cannot be configured: %s (type : %s)", ex.what(), typeid(ex).name());
       parameter_handler_.reset();
       return false;
     }
-    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
-      RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
+    catch (...) {
+      RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring LowPass filter");
       parameter_handler_.reset();
       return false;
     }

include/control_filters/rate_limiter.hpp

@@ -89,13 +89,14 @@ bool RateLimiter<T>::configure()
         std::make_shared<rate_limiter::ParamListener>(this->params_interface_,
                                                          this->param_prefix_);
     }
-    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
-      RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what());
+    catch (const std::exception & ex) {
+      RCLCPP_ERROR((*logger_),
+        "Rate Limiter filter cannot be configured: %s (type : %s)", ex.what(), typeid(ex).name());
       parameter_handler_.reset();
       return false;
     }
-    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
-      RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what());
+    catch (...) {
+      RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring Rate Limiter filter");
       parameter_handler_.reset();
       return false;
     }

test/control_filters/test_exponential_filter.cpp

@@ -12,18 +12,30 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_exponential_filter.hpp"
+#include "test_filter_util.hpp"
 
-TEST_F(ExponentialFilterTest, TestExponentialFilterThrowsUnconfigured)
+#include <memory>
+#include "gmock/gmock.h"
+
+#include "control_filters/exponential_filter.hpp"
+
+TEST_F(FilterTest, TestExponentialFilterThrowsUnconfigured)
 {
     std::shared_ptr<filters::FilterBase<double>> filter_ =
         std::make_shared<control_filters::ExponentialFilter<double>>();
     double in, out;
     ASSERT_THROW(filter_->update(in, out), std::runtime_error);
 }
 
+TEST_F(FilterTest, TestExponentialFilterInvalidParameterValue)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::ExponentialFilter<double>>();
+    ASSERT_FALSE(filter_->configure("", "TestExponentialFilter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+}
 
-TEST_F(ExponentialFilterTest, TestExponentialFilterComputation)
+TEST_F(FilterTest, TestExponentialFilterComputation)
 {
     // parameters should match the test yaml file
     double alpha = 0.7;
@@ -42,13 +54,13 @@ TEST_F(ExponentialFilterTest, TestExponentialFilterComputation)
     ASSERT_EQ(out, 1.0);
 
     // second filter pass with same values: no change
-      // check equality with low-pass-filter
-      ASSERT_TRUE(filter_->update(in, out));
-      calculated = in;
-      ASSERT_EQ(calculated, out);
+    // check equality with low-pass-filter
+    ASSERT_TRUE(filter_->update(in, out));
+    calculated = in;
+    ASSERT_EQ(calculated, out);
 
     // input change
-      in = 0.0;
+    in = 0.0;
     for (int i = 0; i < 100; ++i){
       ASSERT_TRUE(filter_->update(in, out));
       calculated = alpha * in + (1 - alpha) * calculated;

test/control_filters/test_exponential_filter.hpp

@@ -1,3 +1,7 @@
 TestExponentialFilterComputation:
   ros__parameters:
     alpha: 0.7
+
+TestExponentialInvalidParameterValue:
+  ros__parameters:
+    alpha: "a"

test/control_filters/test_exponential_filter_parameters.yaml

@@ -12,52 +12,35 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_
-#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_
+#ifndef CONTROL_FILTERS__TEST_FILTER_UTIL_HPP_
+#define CONTROL_FILTERS__TEST_FILTER_UTIL_HPP_
 
 #include <memory>
-#include <thread>
 #include "gmock/gmock.h"
 
-#include "control_filters/low_pass_filter.hpp"
-#include "geometry_msgs/msg/wrench_stamped.hpp"
-#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/logger.hpp"
 
-namespace
-{
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter");
-}  // namespace
-
-class LowPassFilterTest : public ::testing::Test
+class FilterTest : public ::testing::Test
 {
 public:
   void SetUp() override
   {
     auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name();
     node_ = std::make_shared<rclcpp::Node>(testname);
-    executor_->add_node(node_);
-    executor_thread_ = std::thread([this]() { executor_->spin(); });
   }
 
-  LowPassFilterTest()
+  FilterTest()
   {
-    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
   }
 
   void TearDown() override
   {
-    executor_->cancel();
-    if (executor_thread_.joinable())
-    {
-      executor_thread_.join();
-    }
     node_.reset();
   }
 
 protected:
   rclcpp::Node::SharedPtr node_;
-  rclcpp::Executor::SharedPtr executor_;
-  std::thread executor_thread_;
 };
 
-#endif  // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_
+#endif  // CONTROL_FILTERS__TEST_FILTER_UTIL_HPP_

…control_filters/test_low_pass_filter.hpp test/control_filters/test_filter_util.hpptest/control_filters/test_low_pass_filter.hpp renamed to test/control_filters/test_filter_util.hpp test/control_filters/test_low_pass_filter.hpp renamed to test/control_filters/test_filter_util.hpp

@@ -12,9 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_low_pass_filter.hpp"
+#include "test_filter_util.hpp"
 
-TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters)
+#include <memory>
+#include "gmock/gmock.h"
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+
+#include "control_filters/low_pass_filter.hpp"
+
+TEST_F(FilterTest, TestLowPassWrenchFilterAllParameters)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
         std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
@@ -26,12 +33,13 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters)
     // change a parameter
     node_->set_parameter(rclcpp::Parameter("sampling_frequency", 500.0));
     // accept second call to configure with valid parameters to already configured filter
+    // will give a warning "Filter %s already being reconfigured"
     ASSERT_TRUE(filter_->configure("", "TestLowPassFilter",
         node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
 }
 
 
-TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter)
+TEST_F(FilterTest, TestLowPassWrenchFilterMissingParameter)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
         std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
@@ -41,7 +49,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter)
         node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
 }
 
-TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter)
+TEST_F(FilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
         std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
@@ -57,23 +65,23 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter)
         node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
 }
 
-TEST_F(LowPassFilterTest, TestLowPassFilterThrowsUnconfigured)
+TEST_F(FilterTest, TestLowPassFilterThrowsUnconfigured)
 {
     std::shared_ptr<filters::FilterBase<double>> filter_ =
         std::make_shared<control_filters::LowPassFilter<double>>();
     double in, out;
     ASSERT_THROW(filter_->update(in, out), std::runtime_error);
 }
 
-TEST_F(LowPassFilterTest, TestLowPassWrenchFilterThrowsUnconfigured)
+TEST_F(FilterTest, TestLowPassWrenchFilterThrowsUnconfigured)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
         std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
     geometry_msgs::msg::WrenchStamped in, out;
     ASSERT_THROW(filter_->update(in, out), std::runtime_error);
 }
 
-TEST_F(LowPassFilterTest, TestLowPassWrenchFilterComputation)
+TEST_F(FilterTest, TestLowPassWrenchFilterComputation)
 {
     // parameters should match the test yaml file
     double sampling_freq = 1000.0;