Merge branch 'add-control-filters-gravity-compensation' of https://github.com/StoglRobotics-forks/control_toolbox into add-control-filters-gravity-compensation

Author: dzumkeller

CMakeLists.txt

@@ -21,7 +21,6 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(realtime_tools REQUIRED)
 
@@ -37,7 +36,6 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB
 ament_target_dependencies(${PROJECT_NAME}
     control_msgs
     rclcpp
-    rclcpp_lifecycle
     rcutils
     realtime_tools)
 

control_filters.xml

@@ -1,14 +1,14 @@
 <class_libraries>
-    <library path="gravity_compensation">
-        <class name="control_filters/GravityCompensatorWrench"
-            type="control_filters::GravityCompensator<geometry_msgs::msg::WrenchStamped>"
-            base_class_type="filters::FilterBase<geometry_msgs::msg::WrenchStamped>">
-            <description>
-                This is a gravity compensation filter working with geometry_msgs::WrenchStamped.
-                It subtracts the influence of a force caused by the gravitational force from force
-                measurements.
-            </description>
-        </class>
+  <library path="gravity_compensation">
+    <class name="control_filters/GravityCompensatorWrench"
+           type="control_filters::GravityCompensator<geometry_msgs::msg::WrenchStamped>"
+           base_class_type="filters::FilterBase<geometry_msgs::msg::WrenchStamped>">
+    <description>
+      This is a gravity compensation filter working with geometry_msgs::WrenchStamped.
+      It subtracts the influence of a force caused by the gravitational force from force
+      measurements.
+    </description>
+  </class>
     </library>
     <library path="low_pass_filter">
         <class name="control_filters/LowPassFilterWrench"

include/control_filters/low_pass_filter.hpp

@@ -16,10 +16,11 @@
 #define CONTROL_FILTERS__LOW_PASS_FILTER_HPP
 
 #include <cmath>
+#include <Eigen/Dense>
+
 #include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "filters/filter_base.hpp"
 #include "filters/filter_chain.hpp"
-#include <Eigen/Dense>
 #include "rclcpp/node.hpp"
 
 
@@ -31,14 +32,11 @@ class LowPassFilter : public filters::FilterBase<T>
 public:
     LowPassFilter();
 
-    ~LowPassFilter();
+    ~LowPassFilter() override;
 
-    /** @brief Configure filter parameters  */
-    virtual bool configure();
+    bool configure() override;
 
-    bool configure(const std::string& ns);
-
-    virtual bool update(const T& data_in, T& data_out);
+    bool update(const T& data_in, T& data_out) override;
 
     /** \brief Get most recent parameters */
     bool updateParameters();
@@ -50,7 +48,6 @@ class LowPassFilter : public filters::FilterBase<T>
     rclcpp::Logger logger_;
 
     // Parameters
-    std::string parameter_namespace_;
     double sampling_frequency_;
     double damping_frequency_;
     double damping_intensity_;
@@ -81,21 +78,12 @@ LowPassFilter<T>::~LowPassFilter()
 
 template <typename T>
 bool LowPassFilter<T>::configure()
-{
-    configure("");
-    return true;
-}
-
-template <typename T>
-bool LowPassFilter<T>::configure(const std::string& ns)
 {
     if (!initialized_)
     {
         RCLCPP_INFO(logger_, "Node is not initialized... call setNode()");
         return false;
     }
-
-    parameter_namespace_ = ns;
 
     if (!updateParameters())
     {
@@ -184,19 +172,19 @@ template <typename T>
 bool LowPassFilter<T>::updateParameters()
 {
 
-    if (!filters::FilterBase<T>::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) {
+    if (!filters::FilterBase<T>::getParam("sampling_frequency", sampling_frequency_)) {
         RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency");
         return false;
     }
-    if (!filters::FilterBase<T>::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) {
+    if (!filters::FilterBase<T>::getParam("damping_frequency", damping_frequency_)) {
         RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency");
         return false;
     }    
-    if (!filters::FilterBase<T>::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) {
+    if (!filters::FilterBase<T>::getParam("damping_intensity", damping_intensity_)) {
         RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity");
         return false;
     }    
-    if (!filters::FilterBase<T>::getParam(parameter_namespace_ + "divider", divider_)) {
+    if (!filters::FilterBase<T>::getParam("divider", divider_)) {
         RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider");
         return false;
     }