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).
The current code uses
measured_cpto broadcast to tf2. We could add a second tree usingsetpoint_cp. For this to work we would likely need to add a prefix to all the frame names (in bridgeAddtf2BroadcasterFromCommandRead) and make sure all components havesetpoint_cp(i.e. SUJs).