Simulation Slowdown with PointCloud2 Subscriber

Omniverse Isaac Sim
1

Hello everyone,

I’m currently working on a project using NVIDIA Isaac Sim to synchronize a simulated robot with data from a ROS2 bagfile recorded from a real robot. The synchronization of the robot’s movements works perfectly; however, I encounter significant slowdowns in the simulation as soon as a PointCloud2 subscriber starts receiving messages.

Here’s the code snippet I’m using:

import argparse
from utils.file_operations import read_yaml
from utils.transforms import update_object_dict_with_transforms, process_transform, check_pose_deviation

# ---------------------------------------------------------------------------- #
#                       Read the YAML configuration file                       #
# ---------------------------------------------------------------------------- #
parser = argparse.ArgumentParser()
parser.add_argument("--config_file", help="Path to the configuration file", default="config/params.yaml")
args = parser.parse_args()

config = read_yaml(args.config_file)

# ---------------------------------------------------------------------------- #
#                            Start The SimulationApp                           #
# ---------------------------------------------------------------------------- #
import isaacsim
from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp(launch_config=config.launch_config)

import numpy as np
import warnings
import threading

import omni.client
import omni.kit.commands
import omni.kit.app
import omni.timeline
import omni.usd
from omni.isaac.core import World
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.prims import XFormPrim
from pxr import UsdPhysics, PhysxSchema

# enable ROS2 bridge extension
enable_extension("omni.isaac.ros2_bridge")
simulation_app.update()

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from sensor_msgs_py import point_cloud2

warnings.filterwarnings("ignore")

class IsaacSim(Node):
    def __init__(self):
        super().__init__('IsaacSim')

        # setting up the world
        self.world_setup()

        # load environment and robot
        self.load_env()
        self.load_robot()

        # reset ros world
        self.ros_world.reset()


    def world_setup(self):
        self.timeline = omni.timeline.get_timeline_interface()
        self.ros_world = World(stage_units_in_meters=1.0)
        self.add_physics_scene()


    def add_physics_scene(self):
        self.stage = omni.usd.get_context().get_stage()        
        self.physics_scene = UsdPhysics.Scene.Define(self.stage, "/physicsScene")
        physx_scene = PhysxSchema.PhysxSceneAPI.Apply(self.stage.GetPrimAtPath("/physicsScene"))
        physx_scene.GetEnableCCDAttr().Set(False)
        physx_scene.GetEnableGPUDynamicsAttr().Set(False)
        physx_scene.GetCollisionSystemAttr().Set("PCM")
        physx_scene.GetSolverTypeAttr().Set("TGS")
        physx_scene.GetBroadphaseTypeAttr().Set("MBP")
        physx_scene.GetTimeStepsPerSecondAttr().Set(480)

        
    def load_env(self):
        env_url = "/root/ws/environments/" + config.environment_config.environment_usd
        add_reference_to_stage(usd_path=env_url, prim_path="/Environment")
        self.ros_world.scene.add(XFormPrim(prim_path="/Environment", name="Environment"))


    def load_robot(self):
        robot_url = "/root/ws/robots/USD/" + config.robot_config.robot_usd
        add_reference_to_stage(usd_path=robot_url, prim_path="/kairosAB")
        self.ros_world.scene.add(XFormPrim(prim_path="/kairosAB", name="kairosAB"))
        

    def run_simulation(self):
        self.timeline.play()
        while simulation_app.is_running():
            self.ros_world.step(render=True)            
            if self.ros_world.is_playing():
                pass
     
        # cleanup
        self.timeline.stop()
        self.destroy_node()
        simulation_app.close()


class PointCloudSubscriber(Node):
    
    def __init__(self,):
        
        super().__init__('point_cloud_sub')
        self.subscription_point_cloud = self.create_subscription(PointCloud2, "/kairosAB/ouster/points", self.point_cloud_callback, 100)   


    def point_cloud_callback(self, msg):
        print("Received")
        points_msg = np.array(list(point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)))
        print("Finished processing")
        print("_________________")
        

def main():
    rclpy.init()

    node1 = IsaacSim()
    node2 = PointCloudSubscriber()
    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(node1)
    executor.add_node(node2)

    # Spin in a separate thread
    executor_thread = threading.Thread(target=executor.spin, daemon=True)
    executor_thread.start()

    node1.run_simulation()

    rclpy.shutdown()
    executor_thread.join()

if __name__ == '__main__':
    main()

I’ve tried implementing multi-threading to address the issue, but unfortunately, the problem persists. Below is a video demonstrating how the simulation slows down once the first PointCloud2 message is received and processed:

I would like the PointCloud2 subscriber to operate independently from the simulation to prevent any slowdowns. Does anyone have any suggestions or solutions on how I can achieve this?

Any help would be greatly appreciated!

Thank you,
Lukas