From 8022808904a64398476e9d5d1a456c330c8b8d58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= <denis@stoglrobotics.de>
Date: Mon, 18 Dec 2023 17:58:04 +0100
Subject: [PATCH] Fixup tests after revert.

---
 joint_limits/src/joint_saturation_limiter.cpp | 59 +++++++++++++++----
 .../test/test_joint_saturation_limiter.cpp    |  2 +-
 2 files changed, 47 insertions(+), 14 deletions(-)

diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp
index dcbd9c4a92..9e949ee9be 100644
--- a/joint_limits/src/joint_saturation_limiter.cpp
+++ b/joint_limits/src/joint_saturation_limiter.cpp
@@ -32,7 +32,10 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
 {
   const auto dt_seconds = dt.seconds();
   // negative or null is not allowed
-  if (dt_seconds <= 0.0) return false;
+  if (dt_seconds <= 0.0)
+  {
+    return false;
+  }
 
   // TODO(gwalck) compute if the max are not implicitly violated with the given dt
   // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4
@@ -150,7 +153,9 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
             return true;
           }
           else
+          {
             return false;
+          }
         };
 
         // limit acc for pos_cmd and/or vel_cmd
@@ -314,12 +319,17 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
           current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
       }
       if (has_pos_cmd)
+      {
         desired_pos[index] = current_joint_states.positions[index] +
                              current_joint_states.velocities[index] * dt_seconds +
                              0.5 * desired_acc[index] * dt_seconds * dt_seconds;
+      }
     }
     std::ostringstream ostr;
-    for (auto jnt : limited_jnts_pos) ostr << jnt << " ";
+    for (auto jnt : limited_jnts_pos)
+    {
+      ostr << jnt << " ";
+    }
     ostr << "\b \b";  // erase last character
     RCLCPP_WARN_STREAM_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
@@ -334,7 +344,10 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
   if (limited_jnts_pos.size() > 0)
   {
     std::ostringstream ostr;
-    for (auto jnt : limited_jnts_pos) ostr << jnt << " ";
+    for (auto jnt : limited_jnts_pos)
+    {
+      ostr << jnt << " ";
+    }
     ostr << "\b \b";  // erase last character
     RCLCPP_WARN_STREAM_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
@@ -344,7 +357,10 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
   if (limited_jnts_vel.size() > 0)
   {
     std::ostringstream ostr;
-    for (auto jnt : limited_jnts_vel) ostr << jnt << " ";
+    for (auto jnt : limited_jnts_vel)
+    {
+      ostr << jnt << " ";
+    }
     ostr << "\b \b";  // erase last character
     RCLCPP_WARN_STREAM_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
@@ -354,7 +370,10 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
   if (limited_jnts_acc.size() > 0)
   {
     std::ostringstream ostr;
-    for (auto jnt : limited_jnts_acc) ostr << jnt << " ";
+    for (auto jnt : limited_jnts_acc)
+    {
+      ostr << jnt << " ";
+    }
     ostr << "\b \b";  // erase last character
     RCLCPP_WARN_STREAM_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
@@ -364,23 +383,35 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
   if (limited_jnts_dec.size() > 0)
   {
     std::ostringstream ostr;
-    for (auto jnt : limited_jnts_dec) ostr << jnt << " ";
+    for (auto jnt : limited_jnts_dec)
+    {
+      ostr << jnt << " ";
+    }
     ostr << "\b \b";  // erase last character
     RCLCPP_WARN_STREAM_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
       "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting");
   }
 
-  if (has_pos_cmd) desired_joint_states.positions = desired_pos;
-  if (has_vel_cmd) desired_joint_states.velocities = desired_vel;
-  if (has_acc_cmd) desired_joint_states.accelerations = desired_acc;
+  if (has_pos_cmd)
+  {
+    desired_joint_states.positions = desired_pos;
+  }
+  if (has_vel_cmd)
+  {
+    desired_joint_states.velocities = desired_vel;
+  }
+  if (has_acc_cmd)
+  {
+    desired_joint_states.accelerations = desired_acc;
+  }
   return true;
 }
 
 template <>
 bool JointSaturationLimiter<JointLimits>::on_enforce_effort(
-  trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
-  trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt)
+  trajectory_msgs::msg::JointTrajectoryPoint &,
+  trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration &)
 {
   std::vector<double> desired_effort(number_of_joints_);
   std::vector<std::string> limited_joints_effort;
@@ -415,7 +446,10 @@ bool JointSaturationLimiter<JointLimits>::on_enforce_effort(
   if (limited_joints_effort.size() > 0)
   {
     std::ostringstream ostr;
-    for (auto jnt : limited_joints_effort) ostr << jnt << " ";
+    for (auto jnt : limited_joints_effort)
+    {
+      ostr << jnt << " ";
+    }
     ostr << "\b \b";  // erase last character
     RCLCPP_WARN_STREAM_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
@@ -428,7 +462,6 @@ bool JointSaturationLimiter<JointLimits>::on_enforce_effort(
 }
 
 }  // namespace joint_limits
-   // namespace joint_limits
 
 #include "pluginlib/class_list_macros.hpp"
 
diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp
index e9705a6662..8161481c7f 100644
--- a/joint_limits/test/test_joint_saturation_limiter.cpp
+++ b/joint_limits/test/test_joint_saturation_limiter.cpp
@@ -91,7 +91,7 @@ TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false)
   }
 }
 
-TEST_F(SimpleJointLimiterTest, when_no_velstate_expect_enforce_succeed)
+TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_enforce_succeed)
 {
   SetupNode("joint_saturation_limiter");
   Load();