Port robot_state_helper from ROS1

### Feature summary

Port state helper node from ROS 1

https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/src/robot_state_helper.cpp

### Related issues
Some work has been done in #6

### Tasks
*To complete this issue involves*
- [ ] Implement the feature
- [ ] Make documentation
- [ ] Make Unit test
- [ ] Test on real hardware
