PathController step/loiter doesn't run when DesiredPath is updated

[krisgry]

Looking into PathController again: I realized that if you receive DesiredPath at the same frequency as m_cperiod, step/loiter will not be called at all.

In consume(desiredPath), m_ts.now = now

dune/src/DUNE/Control/PathController.cpp

Lines 378 to 380 in 11e11ec

	m_ts.start_time = now;
	m_ts.end_time = -1;
	m_ts.now = m_ts.start_time;

while consume(EstimatedState) returns before calling step/loiter if the time between "last run" and now is less than m_cperiod:

dune/src/DUNE/Control/PathController.cpp

Lines 658 to 659 in 11e11ec

	if (now < m_ts.now + m_cperiod)
	return;

which causes step/loiter to not be run at all.

I discovered this while working with FollowReference, where I update the Reference at the same frequency as the PathController. This might be a "user problem", since I have little experience with FollowReference (so please correct me if I do something wrong :) ). From a control perspective, of course the inner loop should be faster than the outer loop, but I still think the inner loop should run :) Haven't tried, but I guess this also means that every time you receive a new path, step/loiter will be skipped. Is there a rationale behind this?

Thanks!

[krisgry]

One thing I did not mention; when I increased the frequency of the PathControllers (so that it is faster than my update of IMC::Reference, and thereby DesiredPath), this problem is circumvented.

[krisgry]

Our suspicions have been confirmed; when commenting out lines 658-658 above, this is no longer an issue. My suggestion is then to add some debug printout in the if, so that the user is warned. E.g. something like

if (now < m_ts.now + m_cperiod) 
   {
      debug("Not running step/loiter! Too little time since last run or arrival of new path");
      return;
   }