All this block and its implementation seems copied from the ros robot_state_publisher API. We shouldn't be doing this unless there is a good reason. Why not linking to the robot_state_publisher library and just use it?