Fix publishing only top level model pose in pose publisher

[iche033]

🦟 Bug fix

Fixes #2690

Summary

The pose publisher system should now publish top level model pose with these settings:

        <publish_model_pose>true</publish_model_pose>
        <publish_nested_model_pose>false</publish_nested_model_pose>

This PR preserves the behavior and backward compatibility logic that were added in #1342, i.e.

1. The system should only publish nested model pose with these params:

           <publish_model_pose>false</publish_model_pose>
           <publish_nested_model_pose>true</publish_nested_model_pose>

2. Backward behavior - if <publish_model_pose> is not specified, it defaults to the same value as <publish_nested_model_pose> - we should remove this behavior in gz-sim10 / jetty. I added a todo note for this.

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[azeey]

This is related to #2321. There, I proposed adding a new parameter to publish absolute poses. Any thoughts on that?

[iche033]

This is related to #2321. There, I proposed adding a new parameter to publish absolute poses. Any thoughts on that?

Setting <publish_model_pose>true</publish_model_pose> should now publish absolute poses for the top level model. I remember that was also the intension in #1342 (in addition to letting users disable publishing top model poses) but I didn't add a test for this and so there was this bug.

The confusing part currently is how publish_nested_model_pose can still be used to determine if the top level model pose is published or not (if publish_model_pose is not specified). This was so that it does not break applications like subt. I propose to decouple publish_model_pose and publish_nested_model_pose completely in gz-sim10, i.e. publish_nested_model_pose should not affect whether or not absolute poses are published - it should only affect nested models and ignored if nested models are not present.

[azeey]

Setting <publish_model_pose>true</publish_model_pose> should now publish absolute poses for the top level model.

This is true, but if the plugin is inside a nested model, it would publish relative poses right? I think having a separate flag to publish the absolute pose regardless of the hierarchy of the model would be good to have. I think it would be less confusing.

But I see now that this PR is fixing a bug and I don't want to request a new feature here. I'll keep #2321 open, but won't block this PR.

I propose to decouple publish_model_pose and publish_nested_model_pose completely in gz-sim10, i.e. publish_nested_model_pose should not affect whether or not absolute poses are published - it should only affect nested models and ignored if nested models are not present.

Agreed!

[azeey]

I'm confused about #2690 though. It's saying

  <publish_model_pose>true</publish_model_pose>
  <publish_nested_model_pose>true</publish_nested_model_pose>

doesn't work, but looking at the code, I would expect it to work. @Kametor do you have an example SDF file we can try?

[iche033]

I'm confused about #2690 though. It's saying

  <publish_model_pose>true</publish_model_pose>
  <publish_nested_model_pose>true</publish_nested_model_pose>

doesn't work, but looking at the code, I would expect it to work. @Kametor do you have an example SDF file we can try?

I think it's saying that both of the parameters have to be true in order for the pose to be published?

So this does not work, i.e. (non-nested) model pose was not published:

   <publish_model_pose>true</publish_model_pose>
   <publish_nested_model_pose>false</publish_nested_model_pose>

This line prevented the (non-nested) model pose to be published:

gz-sim/src/systems/pose_publisher/PosePublisher.cc

Line 410 in f0cde05

fillPose = this->publishNestedModelPose && this->publishModelPose;

the logic requires both publish_nested_model_pose and publish_model_pose and to be true in order to publish the model pose.

I used the pose_publisher.sdf example world for manual testing.

[iche033]

I think having a separate flag to publish the absolute pose regardless of the hierarchy of the model would be good to have.

ah ok I see. Yes that sounds good to me.

[azeey]

I see. For some reason, I misread it as "it's not working when both parameters are set to true".

[azeey]

So, if I understand this correctly, the truth table is:

publish_model_pose	publish_nested_model_pose	Top level model is published
T	T	T
T	F	T
T	X	F
F	T	F
F	F	F
F	X	F
X	T	T (This is what we want to change in gz-sim10?)
X	F	F
X	X	F

whereas before, it was

publish_model_pose	publish_nested_model_pose	Top level model is published
T	T	T
T	F	F (This is the only difference)
T	X	F
F	T	F
F	F	F
F	X	F
X	T	T
X	F	F
X	X	F

[iche033]

yes my interpretation is the same as what's described in the tables above.

The code change in gz-sim10 would be:

   this->dataPtr->publishModelPose =
     _sdf->Get<bool>("publish_model_pose",
-        this->dataPtr->publishNestedModelPose).first;
+        this->dataPtr->publishdModelPose).first;

and the entry in that table would become:

publish_model_pose	publish_nested_model_pose	Top level model is published
X	T	F