From c5e1e5d3e6ef84fc1d1d650eb9b2cc7bc4b3e490 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 24 May 2024 18:37:15 +0200
Subject: [PATCH] Avoid to print continuosly the warning about the timings

---
 include/DCMTrajectoryGeneratorHelper.h |  2 ++
 src/DCMTrajectoryGeneratorHelper.cpp   | 12 +++++++++---
 src/ZMPTrajectoryGenerator.cpp         | 11 +++++++++--
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/include/DCMTrajectoryGeneratorHelper.h b/include/DCMTrajectoryGeneratorHelper.h
index ed03b2f..c254fbc 100644
--- a/include/DCMTrajectoryGeneratorHelper.h
+++ b/include/DCMTrajectoryGeneratorHelper.h
@@ -153,6 +153,8 @@ class DCMTrajectoryGeneratorHelper
     std::vector<iDynTree::Vector2> m_ZMPPosition; /**< Vector containing the position of the ZMP. */
     std::vector<double> m_weightInLeft, m_weightInRight; /**< Vectors containing the percentage of weight carried by the foot. */
 
+    bool m_timingWarningPrinted{false}; /**< Flag used to print a warning message about the timings only once. */
+
 
     /**
      * Add the last Single and Double support phases.
diff --git a/src/DCMTrajectoryGeneratorHelper.cpp b/src/DCMTrajectoryGeneratorHelper.cpp
index 690acac..192bf52 100644
--- a/src/DCMTrajectoryGeneratorHelper.cpp
+++ b/src/DCMTrajectoryGeneratorHelper.cpp
@@ -1467,9 +1467,15 @@ bool DCMTrajectoryGeneratorHelper::setPauseConditions(bool pauseActive, const do
             return false;
         }
 
-        if (nominalDoubleSupportDuration + endSwitchTime > maxDoubleSupportDuration){
-            std::cerr << "[DCMTrajectoryGeneratorHelper::setPauseConditions] Warning: the sum of nominalDoubleSupportDuration and endSwitchTime is greater than maxDoubleSupportDuration. ";
-            std::cerr << "The robot might not be able to pause in the middle of the stance phase." << std::endl;
+        if (nominalDoubleSupportDuration + endSwitchTime > maxDoubleSupportDuration) {
+            if (!m_timingWarningPrinted) {
+                std::cerr << "[DCMTrajectoryGeneratorHelper::setPauseConditions] Warning: the sum of nominalDoubleSupportDuration and endSwitchTime is greater than maxDoubleSupportDuration. ";
+                std::cerr << "The robot might not be able to pause in the middle of the stance phase." << std::endl;
+                m_timingWarningPrinted = true;
+            }
+        }
+        else {
+            m_timingWarningPrinted = false;
         }
     }
     m_nominalDoubleSupportDuration = nominalDoubleSupportDuration;
diff --git a/src/ZMPTrajectoryGenerator.cpp b/src/ZMPTrajectoryGenerator.cpp
index 4603294..0d3d2ce 100644
--- a/src/ZMPTrajectoryGenerator.cpp
+++ b/src/ZMPTrajectoryGenerator.cpp
@@ -34,6 +34,7 @@ class ZMPTrajectoryGenerator::ZMPTrajectoryGeneratorImplementation {
     std::vector<iDynTree::Vector2> leftZMPAcceleration, rightZMPAcceleration, worldZMPAcceleration;
 
     bool initialWeightSpecified = false, previousStepsSpecified = false;
+    bool timingWarningPrinted = false;
 
     std::mutex mutex;
 
@@ -519,8 +520,14 @@ bool ZMPTrajectoryGenerator::computeNewTrajectories(double initTime, double dT,
     m_pimpl->endSwitchTime = endSwitchTime;
 
     if (m_pimpl->nominalSwitchTime + endSwitchTime > m_pimpl->maxSwitchTime) {
-        std::cerr << "[ZMPTrajectoryGenerator::computeNewTrajectories] Warning: the sum of nominalSwitchTime and endSwitchTime is greater than maxSwitchTime. ";
-        std::cerr << "The robot might not be able to pause in the middle of the stance phase." << std::endl;
+        if (!m_pimpl->timingWarningPrinted) {
+            std::cerr << "[ZMPTrajectoryGenerator::computeNewTrajectories] Warning: the sum of nominalSwitchTime and endSwitchTime is greater than maxSwitchTime. ";
+            std::cerr << "The robot might not be able to pause in the middle of the stance phase." << std::endl;
+            m_pimpl->timingWarningPrinted = true;
+        }
+    }
+    else {
+        m_pimpl->timingWarningPrinted = false;
     }
 
     if (!(m_pimpl->initialWeightSpecified)) {