From 885985621eb6e8ec501369521204b383b0086839 Mon Sep 17 00:00:00 2001
From: Paul Bovbel <pbovbel@clearpathrobotics.com>
Date: Sat, 30 Apr 2016 13:30:04 -0400
Subject: [PATCH] Use boost clamp

---
 src/pid.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/pid.cpp b/src/pid.cpp
index 0fbe0757..2be23074 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -41,6 +41,8 @@
 #include <control_toolbox/pid.h>
 #include <tinyxml.h>
 
+#include <boost/algorithm/clamp.hpp>
+
 namespace control_toolbox {
 
 static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default prefix?
@@ -318,7 +320,9 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
   if(gains.antiwindup_)
   {
     // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
-    i_error_ = std::max(gains.i_min_ / std::fabs(gains.i_gain_), std::min(i_error_, gains.i_max_ / std::fabs(gains.i_gain_)));
+    i_error_ = boost::algorithm::clamp(i_error_,
+                                       gains.i_min_ / std::abs(gains.i_gain_),
+                                       gains.i_max_ / std::abs(gains.i_gain_));
   }
 
   // Calculate integral contribution to command
@@ -327,7 +331,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
   if(!gains.antiwindup_)
   {
     // Limit i_term so that the limit is meaningful in the output
-    i_term = std::max(gains.i_min_, std::min(i_term, gains.i_max_));
+    i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
   }
 
   // Calculate derivative contribution to command