Call UserCommand set_pose user without SceneBroadcaster triggering a full state update

[srmainwaring]

Desired behaviour

Would like to be able to call the service to set a model pose at a rate higher than the current limit (which is about 5Hz). For example, the user command to manually set the pose of a model box in the world model_move is:

ign service -s /world/move_model/set_pose --reqtype ignition.msgs.Pose  --reptype ignition.msgs.Boolean  --timeout 300 --req 'name: "box", position: {x: 0, y: 0, z: 10}'

The service can be called repeatedly in a small C++ program by adapting the same code used to implement the user command in ign-transport/src/cmd/ign.cc L190-250. I'm using a python script and pybind11 bindings to the ignition transport library to do the same thing. Regardless of the mechanism, there appears to be an upper limit (between 5Hz and 10Hz) to the rate at which the pose of a model can be updated. Any higher and the GUI will spin, and no updates to the visual will occur. The Component Inspector will also stop displaying updates to the Pose and WorldPoseCmd components once this limit is reached.

The problem appears to be that the SceneBroadCaster system requires a full state update here:

https://github.com/ignitionrobotics/ign-gazebo/blob/310668726b81271b5597845ae528c1e3ffa0379a/src/systems/scene_broadcaster/SceneBroadcaster.cc#L332-L336

changeEvent is true because the entity has a one time component change

https://github.com/ignitionrobotics/ign-gazebo/blob/310668726b81271b5597845ae528c1e3ffa0379a/src/systems/scene_broadcaster/SceneBroadcaster.cc#L316-L317

An abridged version of the call stack from the user command is:

• The user command service /world/<world>/set_pose adds a components::WorldPoseCmd to the target entity either by calling ECM::CreateComponent or by updating the pose on an existing component then calling ECM::SetChanged with a state ComponentState::OneTimeChange
• Both routes increment the list of oneTimeChangedComponents held in the ECM
• The Physics system maintains a list of worldPoseCmdsToRemove which it does so after applying the changed poses to the relevant entities. For non-static entities the ignition-physics FreeGroup has it's pose updated, for static entities another ECM::SetChanged call is made, this time for components::Pose.
• The SceneBroadcaster system then emits a message for the updated pose in SceneBroadcaster::PostUpdate but also emits a full state update as described above.

As the rate at which the set_pose service is called increases beyond a certain threshold the system cannot clear the serialisation buffer fast enough (and the GUI appears to no be able to deserialise fast enough) and the system stalls.

It's clear that higher rates of pose updates can be achieved if the full state is not republished because the scene broadcaster already publishes pose updates from the physics engine at 60Hz.

Alternatives considered

Writing a system plugin that duplicates the functionality of the UserComands and Physics systems but avoids setting ComponentState::OneTimeChange each update.

Implementation suggestion

I haven't spotted an easy solution. Initially I thought that we might be able to replace ComponentState::OneTimeChange in

https://github.com/ignitionrobotics/ign-gazebo/blob/310668726b81271b5597845ae528c1e3ffa0379a/src/systems/user_commands/UserCommands.cc#L1352-L1356

with ComponentState::PeriodicChange, but this doesn't solve the problem because the Physics system is continually clearing out the current WorldPoseCmds and calling ECM::CreateComponent implicitly registers a ComponentState::OneTimeChange.

Additional context

We'd like to be able to use Ignition Gazebo as a viewer for 3D objects where the pose (and physics) is supplied by an external agent. The use case under consideration has ArduPilot SITL providing both flight control and flight dynamics using one of the test models. A user would like to visualise the model in 3D and attach a follow camera while using the SITL test case physics model. The vehicle pose can be obtained using pymavlink or dronekit, and the ignition-transport python bindings mentioned above make it straightforward to direct these to ignition.

[ahcorde]

@srmainwaring did you try this PR #1392 ?

[srmainwaring]

@srmainwaring did you try this PR #1392 ?

Thanks @ahcorde, I missed that. Just cherry-picked #1392 into main for Metal and it completely solves the issue - so perfect! I ran a /world/<world>/set_pose update at 100Hz, GUI updating fine with no spinning.

Will close this.

[marc-wittwer]

@srmainwaring
Great that you got it working with an update rate of 100Hz.

I want to simulate a conveyor belt and want to update the position of the items to get the items to move forward.

I am using a phyton script which basically calls a terminal command.

first_part = "ign service -s /world/<world>/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: \""
model_name = "conveyor_belt_item"
second_part = (
    model_name + '", position: {x: '  + str(x) + ",y: " + str(y) + ",z: " + str(z) + "}'"
)
os.system(first_part + second_part)

However, as you mentioned calling the /world/<world>/set_pose service via the command line with a high frequency is way too slow.

How exactly did you manage to update the pose at a high rate?
Could you send me a code snippet or point me in the right direction?