From 166a9f5e182130507fa04b94f6f7b276584be9be Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= <denis@stoglrobotics.de>
Date: Sun, 3 Dec 2023 20:03:36 +0100
Subject: [PATCH] Correct compilation and tests.

---
 pid_controller/CMakeLists.txt               |  1 +
 pid_controller/package.xml                  |  1 +
 pid_controller/src/pid_controller.cpp       | 12 ++++++++++--
 pid_controller/test/test_pid_controller.cpp | 12 ++++++++----
 pid_controller/test/test_pid_controller.hpp |  2 +-
 5 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt
index d13bda3751..81cbe6f006 100644
--- a/pid_controller/CMakeLists.txt
+++ b/pid_controller/CMakeLists.txt
@@ -6,6 +6,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  angles
   control_msgs
   control_toolbox
   controller_interface
diff --git a/pid_controller/package.xml b/pid_controller/package.xml
index ccfe28aba3..ccbb4b2feb 100644
--- a/pid_controller/package.xml
+++ b/pid_controller/package.xml
@@ -11,6 +11,7 @@
 
   <build_depend>generate_parameter_library</build_depend>
 
+  <depend>angles</depend>
   <depend>control_msgs</depend>
   <depend>control_toolbox</depend>
   <depend>controller_interface</depend>
diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp
index c46e9dbab8..373e941d96 100644
--- a/pid_controller/src/pid_controller.cpp
+++ b/pid_controller/src/pid_controller.cpp
@@ -22,6 +22,7 @@
 #include <string>
 #include <vector>
 
+#include "angles/angles.h"
 #include "control_msgs/msg/single_dof_state.hpp"
 #include "controller_interface/helpers.hpp"
 
@@ -446,7 +447,7 @@ controller_interface::return_type PidController::update_and_write_commands(
       {
         // for continuous angles the error is normalized between -pi<error<pi
         error =
-          angles::shortest_angular_distance(reference_interfaces_[i] - measured_state_values_[i]);
+          angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]);
       }
 
       // checking if there are two interfaces
@@ -488,7 +489,14 @@ controller_interface::return_type PidController::update_and_write_commands(
       {
         state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i];
       }
-      state_publisher_->msg_.dof_states[i].error = error;
+      state_publisher_->msg_.dof_states[i].error =
+        reference_interfaces_[i] - measured_state_values_[i];
+      if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound)
+      {
+        // for continuous angles the error is normalized between -pi<error<pi
+        state_publisher_->msg_.dof_states[i].error =
+          angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]);
+      }
       if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
       {
         state_publisher_->msg_.dof_states[i].error_dot =
diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp
index bbb669dd78..11f703a1a4 100644
--- a/pid_controller/test/test_pid_controller.cpp
+++ b/pid_controller/test/test_pid_controller.cpp
@@ -38,7 +38,6 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success)
   ASSERT_TRUE(controller_->params_.command_interface.empty());
   ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty());
   ASSERT_FALSE(controller_->params_.use_external_measured_states);
-  ASSERT_FALSE(controller_->params_.runtime_param_update);
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
@@ -60,7 +59,6 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success)
     controller_->params_.reference_and_state_interfaces,
     testing::ElementsAreArray(state_interfaces_));
   ASSERT_FALSE(controller_->params_.use_external_measured_states);
-  ASSERT_FALSE(controller_->params_.runtime_param_update);
 }
 
 TEST_F(PidControllerTest, check_exported_intefaces)
@@ -161,11 +159,17 @@ TEST_F(PidControllerTest, reactivate_success)
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
   ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
+  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
+  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp
index 48ffccd737..ab32f5cb48 100644
--- a/pid_controller/test/test_pid_controller.hpp
+++ b/pid_controller/test/test_pid_controller.hpp
@@ -146,7 +146,7 @@ class PidControllerFixture : public ::testing::Test
 protected:
   void SetUpController(const std::string controller_name = "test_pid_controller")
   {
-    ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK);
+    ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK);
 
     std::vector<hardware_interface::LoanedCommandInterface> command_ifs;
     command_itfs_.reserve(dof_names_.size());