Only call generator if motion model history empty

[efernandez]

Changelog

This simplifies the changes from #154 by doing:

• Only call generator if motion model history empty.
• Assert beginning stamp < ending stamp. Or beginning stamp == ending stamp for the special case when the state history is empty.
• Handle the dt == 0 special case in motion model, so no variables or constraints are generator, and no prediction is performed, or the same state is inserted twice into the history.
• Revert the changes to the test_timestamp_manager.cpp unit test
• Add a new test for the case when the transaction has a single involved stamp, since that was precisely the case not handled. Indeed, that test fails w/o Fix Unicycle2DIgnition set_pose #154 or this PR.
• [minor] Declare state2 closer to where it is used.

The special case mentioned above is always the same.

From my tests locally, with a bunch of prints, I confirmed the state history already have the state for the given stamp, so IIRC that's all that was required for #154 . The assert added in this PR enforces the generator function isn't called for those cases it did before, so I think that constitutes a test for this change.

[efernandez]

@svwilliams @ayrton04 for review

[svwilliams]

I like this better than always adding a dummy entry at the end. Thanks.

[efernandez]

I like this better than always adding a dummy entry at the end. Thanks.

And the unit test changes I did in #154 aren't needed any more. Indeed, I reverted all those changes and added a new test. 😃

[efernandez]

FYI Note that it's important to get this PR in now that #180 has been merged, because you'll see ROS messages complaining of zero process noise covariance matrices. They'll be spamming at 0.1Hz, because they're throttled, but that could still be a lot of spam.

That's precisely the case of dt == 0 that is handled as special case in this PR, that also makes dt == 0 happen once per set pose or startup, as opposed to almost constantly.