Broadcast setpoint_cp to tf #192
Description
The current code uses measured_cp to broadcast to tf2. We could add a second tree using setpoint_cp. For this to work we would likely need to add a prefix to all the frame names (in bridge Addtf2BroadcasterFromCommandRead) and make sure all components have setpoint_cp (i.e. SUJs).