Motion control handle jumps when switching fixed reference frame in Rviz2

[Hydran00]

Hi,
I noticed that the motion control handle jumps in a wrong pose when I change the Fixed Frame under Global Options in Rviz2. This happens as soon as I try to move the handle after switching the fixed frame. I managed to solve this error with minor modification accounting for the header of the feedback message coming from the handle when the

m_server->setPose(...)

is called in the updateMotionControlCallback function.
I also added a check on the frame_id in the update function to prevent the selection of a non-fixed frame.

[stefanscherzinger]

Thanks @Hydran00 for spotting this! I'll have a look.

[stefanscherzinger]

@Hydran00
I merged this in #213.
Notice that I couldn't merge your PR directly, because code formatting was needed and you didn't allow changes on your fork, more info here.