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?

[scybd]

@marc-wittwer
Hi, I met the same issue, do you know how to update the pose at a high rate now? :-)

[marc-wittwer]

@scybd Unfortunately not, I ended up using this approach:

Toggle for code:

from .models.ConveyorBelt import ConveyorBelt
from .models.ConveyorBeltItem import ConveyorBeltItem
from .models.PlanningScene import PlanningScene
from .models.CollisionPlane import CollisionPlane
from .models.CollisionPlaneAction import CollisionPlaneAction
from os import path, system
from typing import List
import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Pose, Point, Vector3
from global_state_msgs.msg import CollisionPlane as CollisionPlaneMessage
from global_state_msgs.msg import PlanningScene as PlanningSceneMessage
from global_state_msgs.msg import ConveyorBeltItem as ConveyorBeltItemMessage
from global_state_msgs.msg import TaskUpdate as TaskUpdateMessage


class GlobalStateManager(Node):
    def __init__(self):
        super().__init__("main")

        self._callback_group = ReentrantCallbackGroup()

        self.create_subscription(
            msg_type=CollisionPlaneMessage,
            topic="/collision_plane",
            callback=self.handle_collision_plane_message,
            qos_profile=QoSProfile(depth=1),
            callback_group=self._callback_group,
        )

        self.create_subscription(
            msg_type=TaskUpdateMessage,
            topic="/task_update",
            callback=self.handle_task_update_message,
            qos_profile=QoSProfile(depth=1),
            callback_group=self._callback_group,
        )

        self.planning_scene = PlanningScene()
        self.planning_scene.task_start_pose.position.z = 0.2
        self.planning_scene.task_start_pose.position.x = -0.5
        self.planning_scene.task_start_pose.orientation.w = 0.0
        self.planning_scene.task_start_pose.orientation.x = 1.0

        self.planning_scene.task_end_pose.position.z = 0.4
        self.planning_scene.task_end_pose.position.x = -0.5
        self.planning_scene.task_end_pose.orientation.w = 0.0
        self.planning_scene.task_end_pose.orientation.x = 1.0

        self.publisher_ = self.create_publisher(PlanningSceneMessage, "global_planning_scene", 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.handle_publish_planning_scene)

        conveyor_belt = ConveyorBelt()

        for i in range(0, 5):
            x = 0.25
            y = i * 0.4
            z = 0.1
            name = "conveyor_belt_item" + str(i)
            pose = Pose(position=Point(x=x, y=y, z=z))
            dimensions = Vector3(x=0.1, y=0.1, z=0.2)
            item = ConveyorBeltItem(name, pose, dimensions)
            # TODO: Uncomment to show conveyor belt items
            # self.planning_scene.conveyor_belt.add_item(item)

        # Gazebo
        if "conveyor_belt_item" not in str(scene_info):
            for item in self.planning_scene.conveyor_belt.items:
                print(item.name)
                spawn_conveyor_belt_item_gazebo(item)

        # Start conveyor belt animation
        # # TODO: Uncomment to show conveyor belt items
        # timer_period = 0.03  # seconds
        # self.conveyor_belt_timer = self.create_timer(timer_period, self.handle_move_conveyor_belt_items)

    def handle_collision_plane_message(self, msg: CollisionPlaneMessage):
        print('NEW MESSAGE:', msg)
        if msg.action is CollisionPlaneAction.ADD.value:
            plane = CollisionPlane.create_from_message(msg)
            self.planning_scene.add_plane(plane)

        if msg.action is CollisionPlaneAction.UPDATE.value:
            print('UPDATE received')
            plane = CollisionPlane.create_from_message(msg)
            if any(p.id == msg.id for p in self.planning_scene.planes):
                self.planning_scene.update_plane(plane)
            else:
                self.planning_scene.add_plane(plane)

        if msg.action is CollisionPlaneAction.DELETE.value:
            self.planning_scene.delete_plane(msg.id)

    def handle_task_update_message(self, msg: TaskUpdateMessage):
        print('TASK_UPDATE:', msg)
        if msg.pose_type == 0:
            self.planning_scene.task_start_pose = msg.pose
        if msg.pose_type == 1:
            self.planning_scene.task_end_pose = msg.pose
        if msg.pose_type == 2:
            self.planning_scene.task_with_fixed_orientation = msg.fixed_orientation
        if msg.pose_type == 3:
            self.planning_scene.task_with_fixed_velocity = msg.fixed_velocity
            self.planning_scene.task_velocity = msg.task_velocity

    def handle_publish_planning_scene(self):
        msg = PlanningSceneMessage()
        planes: List[CollisionPlaneMessage] = []
        for plane in self.planning_scene.planes:
            plane_msg = CollisionPlaneMessage()
            plane_msg.id = plane.id
            plane_msg.action = 0
            plane_msg.pose = plane.pose
            plane_msg.dimensions = plane.dimensions
            planes.append(plane_msg)
        msg.planes = planes

        conveyor_belt_items: List[ConveyorBeltItemMessage] = []
        for item in self.planning_scene.conveyor_belt.items:
            item_msg = ConveyorBeltItemMessage()
            item_msg.id = item.name
            item_msg.pose = item.pose
            item_msg.dimensions = item.dimensions
            conveyor_belt_items.append((item_msg))
        msg.conveyor_belt_items = conveyor_belt_items

        msg.task_start_pose = self.planning_scene.task_start_pose
        msg.task_end_pose = self.planning_scene.task_end_pose

        msg.fixed_orientation = self.planning_scene.task_with_fixed_orientation
        msg.fixed_velocity = self.planning_scene.task_with_fixed_velocity
        msg.task_velocity = self.planning_scene.task_velocity

        self.publisher_.publish(msg)

    def handle_move_conveyor_belt_items(self):
        self.planning_scene.conveyor_belt.move_items_forward()


def spawn_conveyor_belt_item_gazebo(item: ConveyorBeltItem):
    model_name = path.join(path.dirname(path.realpath(__file__)), "assets/cylinder")
    first_part = "ign service -s /world/ign_moveit2_follow_target_world/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 300 --req 'sdf_filename:''\""
    second_part = model_name + '.sdf" pose: {position: {'
    third_part = (
            " x:"
            + str(item.pose.position.x)
            + ", y: "
            + str(item.pose.position.y)
            + ", z: "
            + str(item.pose.position.z)
            + '}} ,name: "'
            + item.name
            + "\", allow_renaming: true ' &"
    )

    add_model = first_part + second_part + third_part
    system(add_model)


def set_position_conveyor_belt_item_gazebo(x, y, z, model_name="conveyor_belt_item"):
    first_part = "ign service -s /world/ign_moveit2_follow_target_world/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: \""
    # model_name = "conveyor_belt_item"
    # z = random.random() * 10+3
    # second_part = model_name + "\", position: {z: 5.0}'"
    second_part = (
            model_name
            + '", position: {x: '
            + str(x)
            + ",y: "
            + str(y)
            + ",z: "
            + str(z)
            + "}'"
    )
    system(first_part + second_part)


def main(args=None):
    rclpy.init(args=args)
    state_manager = GlobalStateManager()
    executor = rclpy.executors.MultiThreadedExecutor(14)
    executor.add_node(state_manager)
    executor.spin()

    rclpy.shutdown()
    exit(0)


if __name__ == '__main__':
    main()