Merge pull request #1 from team-diana/messenger-diagnostic-develop

Messenger diagnostic develop
Add rostest
Add cli arguments to test