Merge pull request #68 from GiulioRomualdi/fix/is_stance_phase

Fix the stance phase machinery

Author: S-Dafarra

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

@@ -666,7 +666,9 @@ bool TrajectoryGenerator::getIsStancePhase(std::vector<bool>& isStancePhase)
         {
             // decreased the counter only if it is different from zero.
             // it is required to add a delay in the beginning of the stance phase
-            stancePhaseDelayCounter = stancePhaseDelayCounter == 0 ? 0 : stancePhaseDelayCounter--;
+            stancePhaseDelayCounter = (stancePhaseDelayCounter == 0)
+                                          ? 0
+                                          : (stancePhaseDelayCounter - 1);
 
             // the delay expired the robot can be considered stance
             if(stancePhaseDelayCounter == 0)

src/WalkingModule/src/Module.cpp

@@ -84,6 +84,9 @@ bool WalkingModule::advanceReferenceSignals()
     m_comHeightVelocity.pop_front();
     m_comHeightVelocity.push_back(m_comHeightVelocity.back());
 
+    m_isStancePhase.pop_front();
+    m_isStancePhase.push_back(m_isStancePhase.back());
+
     // at each sampling time the merge points are decreased by one.
     // If the first merge point is equal to 0 it will be dropped.
     // A new trajectory will be merged at the first merge point or if the deque is empty