[9-Feature] Add some unit test framework

[JoshuaGabriel]

Is your feature request related to a problem? Please describe.
This is a bit more open-ended issue, as of now we are lacking test and I want a proposal on what kind of test framework, or what that would even look like.

Describe the solution you'd like
Some solutions to look at are rostests and a python test framework

Additional context