add systems_test_node.cpp to use for unit testing of robot systems and modes