Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

* 'master' of https://gitlab.com/ArmarX/RobotAPI:
  made mutex in PID controller optional

source/RobotAPI/libraries/core/PIDController.cpp

@@ -31,7 +31,7 @@
 using namespace armarx;
 
 
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless) :
+PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, bool threadSafe) :
     Kp(Kp),
     Ki(Ki),
     Kd(Kd),
@@ -44,23 +44,36 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
     controlValueDerivation(0),
     maxControlValue(maxControlValue),
     maxDerivation(maxDerivation),
-    limitless(limitless)
+    limitless(limitless),
+    threadSafe(threadSafe)
 {
     reset();
 }
 
 void PIDController::reset()
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     firstRun = true;
     previousError = 0;
     integral = 0;
     lastUpdateTime = TimeUtil::GetTime();
 }
 
+ScopedRecursiveLockPtr PIDController::getLock() const
+{
+    if (threadSafe)
+    {
+        return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+    }
+    else
+    {
+        return ScopedRecursiveLockPtr();
+    }
+}
+
 void PIDController::update(double measuredValue, double targetValue)
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     IceUtil::Time now = TimeUtil::GetTime();
 
     if (firstRun)
@@ -76,25 +89,25 @@ void PIDController::update(double measuredValue, double targetValue)
 
 void PIDController::update(double measuredValue)
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     update(measuredValue, target);
 }
 
 void PIDController::setTarget(double newTarget)
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     target = newTarget;
 }
 
 void PIDController::setMaxControlValue(double newMax)
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     maxControlValue = newMax;
 }
 
 void PIDController::update(double deltaSec, double measuredValue, double targetValue)
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     processValue = measuredValue;
     target = targetValue;
 
@@ -144,7 +157,7 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
 
 double PIDController::getControlValue() const
 {
-    ScopedRecursiveLock lock(mutex);
+    ScopedRecursiveLockPtr lock = getLock();
     return controlValue;
 }
 

source/RobotAPI/libraries/core/PIDController.h

@@ -39,7 +39,7 @@ namespace armarx
                       float Kd,
                       double maxControlValue = std::numeric_limits<double>::max(),
                       double maxDerivation = std::numeric_limits<double>::max(),
-                      bool limitless = false);
+                      bool limitless = false, bool threadSafe = true);
         void update(double deltaSec, double measuredValue, double targetValue);
         void update(double measuredValue, double targetValue);
         void update(double measuredValue);
@@ -62,6 +62,9 @@ namespace armarx
         double maxDerivation;
         bool firstRun;
         bool limitless;
+        bool threadSafe = true;
+    private:
+        ScopedRecursiveLockPtr getLock() const;
         mutable RecursiveMutex mutex;
     };
     typedef boost::shared_ptr<PIDController> PIDControllerPtr;