Revision history [back]

Without looking at the code, I think what you really want is the tree to be as it is, but remove the stabilized frame as a child of map.

If you have the stabilized frame there, then the normal base link frame has to go through a reduction in dimensionality and add it back in for 3D orientation.

Instead, add the stabilized frame as a child of the base link, so it now has 2 children: laser and stabilized. Then you can take the 3DOF pose in base link and then "stabilize" it as a frame of its own.