Templated pidROS class

Signed-off-by: ahcorde <[email protected]>

CMakeLists.txt

@@ -6,6 +6,10 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -14,7 +18,6 @@ find_package(realtime_tools REQUIRED)
 add_library(${PROJECT_NAME} SHARED
   src/dither.cpp
   src/limited_proxy.cpp
-  src/pid_ros.cpp
   src/pid.cpp
   src/sine_sweep.cpp
   src/sinusoid.cpp)

include/control_toolbox/pid_ros.hpp

@@ -48,10 +48,10 @@
 
 #include "control_toolbox/pid.hpp"
 
-
 namespace control_toolbox
 {
 
+template<typename NodeT>
 class PidROS
 {
 public:
@@ -64,7 +64,7 @@ class PidROS
    * \param node ROS node
    * \param topic_prefix prefix to add to the pid parameters.
    */
-  explicit PidROS(rclcpp::Node::SharedPtr & node, std::string topic_prefix = std::string(""));
+  explicit PidROS(std::shared_ptr<NodeT> node_ptr, std::string topic_prefix = std::string(""));
 
   /*!
    * \brief Destructor of PidROS class.
@@ -166,7 +166,7 @@ class PidROS
 
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
 
-  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<NodeT> node_;
 
   std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::PidState>> rt_state_pub_;
   std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> state_pub_;
@@ -177,4 +177,9 @@ class PidROS
 
 }  // namespace control_toolbox
 
+#ifndef CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
+// Template implementations
+#include "pid_ros_impl.hpp"
+#endif
+
 #endif  // CONTROL_TOOLBOX__PIDROS_HPP_

src/pid_ros.cpp → include/control_toolbox/pid_ros_impl.hpp

@@ -31,6 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
+#ifndef CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
+#define CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
 
 #include <algorithm>
 #include <cmath>
@@ -43,22 +45,25 @@
 namespace control_toolbox
 {
 
-PidROS::PidROS(rclcpp::Node::SharedPtr & node, std::string topic_prefix)
-: node_(node)
+template<typename NodeT>
+PidROS<NodeT>::PidROS(std::shared_ptr<NodeT> node_ptr, std::string topic_prefix)
+:node_(node_ptr)
 {
   if (topic_prefix.back() != '.' && !topic_prefix.empty()) {
     topic_prefix_ = topic_prefix + ".";
   } else {
     topic_prefix_ = topic_prefix;
   }
 
-  state_pub_ = node->create_publisher<control_msgs::msg::PidState>(
+  state_pub_ =  node_->create_publisher<control_msgs::msg::PidState>(
     topic_prefix + "/pid_state", rclcpp::SensorDataQoS());
   rt_state_pub_.reset(
     new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
 }
 
-bool PidROS::getBooleanParam(const std::string & param_name, bool & value)
+template<typename NodeT>
+bool
+PidROS<NodeT>::getBooleanParam(const std::string & param_name, bool & value)
 {
   rclcpp::Parameter param;
   if (node_->has_parameter(param_name)) {
@@ -74,7 +79,9 @@ bool PidROS::getBooleanParam(const std::string & param_name, bool & value)
   }
 }
 
-bool PidROS::getDoubleParam(const std::string & param_name, double & value)
+template<typename NodeT>
+bool
+PidROS<NodeT>::getDoubleParam(const std::string & param_name, double & value)
 {
   rclcpp::Parameter param;
   if (node_->has_parameter(param_name)) {
@@ -96,7 +103,9 @@ bool PidROS::getDoubleParam(const std::string & param_name, double & value)
   }
 }
 
-bool PidROS::initPid()
+template<typename NodeT>
+bool
+PidROS<NodeT>::initPid()
 {
   double p, i, d, i_min, i_max;
   bool antiwindup = false;
@@ -118,14 +127,18 @@ bool PidROS::initPid()
   return all_params_available;
 }
 
-void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value)
+template<typename NodeT>
+void
+PidROS<NodeT>::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value)
 {
   if (!node_->has_parameter(param_name)) {
     node_->declare_parameter(param_name, param_value);
   }
 }
 
-void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+template<typename NodeT>
+void
+PidROS<NodeT>::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
 {
   pid_.initPid(p, i, d, i_max, i_min, antiwindup);
 
@@ -139,25 +152,33 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b
   setParameterEventCallback();
 }
 
-void PidROS::reset()
+template<typename NodeT>
+void
+PidROS<NodeT>::reset()
 {
   pid_.reset();
 }
 
-double PidROS::computeCommand(double error, rclcpp::Duration dt)
+template<typename NodeT>
+double
+PidROS<NodeT>::computeCommand(double error, rclcpp::Duration dt)
 {
   double cmd_ = pid_.computeCommand(error, dt.nanoseconds());
   publishPIDState(cmd_, error, dt);
 
   return cmd_;
 }
 
-Pid::Gains PidROS::getGains()
+template<typename NodeT>
+Pid::Gains
+PidROS<NodeT>::getGains()
 {
   return pid_.getGains();
 }
 
-void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+template<typename NodeT>
+void
+PidROS<NodeT>::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
 {
   node_->set_parameters(
     {
@@ -173,7 +194,9 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min,
   pid_.setGains(p, i, d, i_max, i_min, antiwindup);
 }
 
-void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt)
+template<typename NodeT>
+void
+PidROS<NodeT>::publishPIDState(double cmd, double error, rclcpp::Duration dt)
 {
   Pid::Gains gains = pid_.getGains();
 
@@ -201,17 +224,23 @@ void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt)
   }
 }
 
-void PidROS::setCurrentCmd(double cmd)
+template<typename NodeT>
+void
+PidROS<NodeT>::setCurrentCmd(double cmd)
 {
   pid_.setCurrentCmd(cmd);
 }
 
-double PidROS::getCurrentCmd()
+template<typename NodeT>
+double
+PidROS<NodeT>::getCurrentCmd()
 {
   return pid_.getCurrentCmd();
 }
 
-void PidROS::getCurrentPIDErrors(double & pe, double & ie, double & de)
+template<typename NodeT>
+void
+PidROS<NodeT>::getCurrentPIDErrors(double & pe, double & ie, double & de)
 {
   double _pe, _ie, _de;
   pid_.getCurrentPIDErrors(_pe, _ie, _de);
@@ -220,7 +249,9 @@ void PidROS::getCurrentPIDErrors(double & pe, double & ie, double & de)
   de = _de;
 }
 
-void PidROS::printValues()
+template<typename NodeT>
+void
+PidROS<NodeT>::printValues()
 {
   Pid::Gains gains = pid_.getGains();
 
@@ -229,7 +260,7 @@ void PidROS::printValues()
 
   RCLCPP_INFO_STREAM(
     node_->get_logger(),
-    "Current Values of PID Class:\n" <<
+    "Current Values of PID template:\n" <<
       "  P Gain:       " << gains.p_gain_ << "\n" <<
       "  I Gain:       " << gains.i_gain_ << "\n" <<
       "  D Gain:       " << gains.d_gain_ << "\n" <<
@@ -243,12 +274,16 @@ void PidROS::printValues()
   );
 }
 
-void PidROS::setGains(const Pid::Gains & gains)
+template<typename NodeT>
+void
+PidROS<NodeT>::setGains(const Pid::Gains & gains)
 {
   pid_.setGains(gains);
 }
 
-void PidROS::setParameterEventCallback()
+template<typename NodeT>
+void
+PidROS<NodeT>::setParameterEventCallback()
 {
   auto on_parameter_event_callback = [this](const std::vector<rclcpp::Parameter> & parameters) {
       rcl_interfaces::msg::SetParametersResult result;
@@ -291,7 +326,10 @@ void PidROS::setParameterEventCallback()
     };
 
   parameter_callback_ =
-    node_->add_on_set_parameters_callback(on_parameter_event_callback);
+    node_->get_node_parameters_interface()->add_on_set_parameters_callback(
+      on_parameter_event_callback);
 }
 
 }  // namespace control_toolbox
+
+#endif  // CONTROL_TOOLBOX__PIDROS_IMPL_HPP_

src/pid.cpp

@@ -209,9 +209,6 @@ double Pid::getCurrentCmd()
 
 void Pid::getCurrentPIDErrors(double & pe, double & ie, double & de)
 {
-  // Get the gain parameters from the realtime buffer
-  Gains gains = *gains_buffer_.readFromRT();
-
   pe = p_error_;
   ie = i_error_;
   de = d_error_;

test/pid_parameters_tests.cpp

@@ -15,7 +15,7 @@
 
 #include <memory>
 
-#include "control_toolbox/pidROS.hpp"
+#include "control_toolbox/pid_ros.hpp"
 
 #include "rclcpp/executors.hpp"
 #include "rclcpp/node.hpp"
@@ -28,7 +28,7 @@ TEST(PidParametersTest, InitPidTest)
 {
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
 
-  control_toolbox::PidROS pid(node);
+  control_toolbox::PidROS<rclcpp::Node> pid(node);
 
   const double P = 1.0;
   const double I = 2.0;
@@ -73,7 +73,7 @@ TEST(PidParametersTest, InitPidWithAntiwindupTest)
 {
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
 
-  control_toolbox::PidROS pid(node);
+  control_toolbox::PidROS<rclcpp::Node> pid(node);
 
   const double P = 1.0;
   const double I = 2.0;
@@ -118,7 +118,7 @@ TEST(PidParametersTest, SetParametersTest)
 {
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
 
-  control_toolbox::PidROS pid(node);
+  control_toolbox::PidROS<rclcpp::Node> pid(node);
 
   const double P = 1.0;
   const double I = 2.0;
@@ -166,7 +166,7 @@ TEST(PidParametersTest, GetParametersTest)
 {
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
 
-  control_toolbox::PidROS pid(node);
+  control_toolbox::PidROS<rclcpp::Node> pid(node);
 
   const double P = 1.0;
   const double I = 2.0;
@@ -203,14 +203,13 @@ TEST(PidParametersTest, GetParametersFromParams)
 {
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
 
-  control_toolbox::PidROS pid(node);
+  control_toolbox::PidROS<rclcpp::Node> pid(node);
 
   const double P = 1.0;
   const double I = 2.0;
   const double D = 3.0;
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
-  const bool ANTIWINDUP = true;
 
   ASSERT_FALSE(pid.initPid());
 

test/pid_publisher_tests.cpp

@@ -18,13 +18,14 @@
 #include <memory>
 #include <thread>
 
-#include "control_toolbox/pidROS.hpp"
+#include "control_toolbox/pid_ros.hpp"
 
 #include "gtest/gtest.h"
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/executors.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/utilities.hpp"
 
 using PidStateMsg = control_msgs::msg::PidState;
@@ -37,7 +38,7 @@ TEST(PidPublihserTest, PublishTest)
 
   auto node = std::make_shared<rclcpp::Node>("pid_publisher_test");
 
-  control_toolbox::PidROS pid_ros(node);
+  control_toolbox::PidROS<rclcpp::Node> pid_ros(node);
 
   pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
 
@@ -56,7 +57,7 @@ TEST(PidPublihserTest, PublishTest)
 
   // wait for callback
   for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) {
-    double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0));
+    pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0));
     rclcpp::spin_some(node);
     std::this_thread::sleep_for(DELAY);
   }

test/pid_tests.cpp

@@ -124,7 +124,6 @@ TEST(ParameterTest, integrationAntiwindupTest)
   Pid pid(0.0, i_gain, 0.0, i_max, i_min, true);
 
   double cmd = 0.0;
-  double pe, ie, de;
 
   cmd = pid.computeCommand(-1.0, 1.0);
   EXPECT_EQ(-1.0, cmd);
@@ -151,7 +150,6 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest)
   Pid pid(0.0, i_gain, 0.0, i_max, i_min, true);
 
   double cmd = 0.0;
-  double pe, ie, de;
 
   cmd = pid.computeCommand(0.1, 1.0);
   EXPECT_EQ(-0.2, cmd);