ROS2 Custom Action Graph Creation

[rishavpathak23]

Hi, has anyone created a custom ros action graph in Isaac Sim (ROS2) , I was following the tutorial for custom python action graph, but am stuck . If anyone has implemented the same can you please share a snippet for a ros2 subscription?
Below is my attached code .

import numpy
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Twist
# from omni.isaac.core_nodes import BaseWriterNode,

# global called
called = False

print(called)


class OgnRos2AMCL(Node):
    """
         This node will subscribe to amcl_pose (ros2-node)
    """
    #
    def __init__(self):
        super().__init__('amcl_subscriber_node')
        self.called = False
    
    def intialize_once(self):
        if not self.called:
            print('Initializing')
            rclpy.init()
            self.called = True

    # called = True
    # @staticmethod
    def compute(self, db) -> bool:
        """Compute the outputs from the current input"""
        global called

        try:
            # if not called:
            #     print("Initialized called",called)
            #     called = True
            #     print("Initialized called",called)
            #     rclpy.init()

            # With the compute in a try block you can fail the compute by raising an exception
            
            self.intialize_once()
            topicName = db.inputs.topicName

            print("Topic Name - ",topicName)

            def callback(msg):
                print(msg)


            node = rclpy.create_node('amcl_subscriber')
            # rclpy.spin(node)
            sub = node.create_subscription(Twist, topicName,callback,10 )
            print(sub)
            rclpy.spin(node)
            pass
        except Exception as error:
            db.log_error(error)
            return False

        # Even if inputs were edge cases like empty arrays, correct outputs mean success
        return True

The problem is whenver I play and have this node in Isaac sim it gives an error saying
1, Cannot create a node (rclpy.init() must be called once)
2. Or rclypy.init() must be initialized only once
The error depends were I initialize the init() function. The latter error comes when I initialize the init() funtion in the compute() method.

The inputs for the node is - exec_in(type - execution), and topicName(type - string)
Outputs - orientation(quatd) and position(vectord)

The node is not been created and itis not able to subscribe

Can anyone please help me with what am I doing wrong

[Ayush_G]

Hello @rishavpathak23,

Here is a minimalist example of how you can use a ROS2 subscriber with a custom Omnigraph Python node.
I first setup the node as such:

Once you populate the extension, save the node as OgnROS2CustomPythonNode.ogn. Then click on “Generate Blank Implementation” followed by “Edit Node”.

You can then replace the contents of the node python file with the following snippet:

# Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved.
#
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto. Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.
#

"""
This is the implementation of the OGN node defined in OgnROS2CustomPythonNode.ogn
"""

# Array or tuple values are accessed as numpy arrays so you probably need this import
import numpy

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from omni.new.extension.ogn.OgnROS2CustomPythonNodeDatabase import OgnROS2CustomPythonNodeDatabase


class OgnROS2CustomPythonNodeInternalState():
    def __init__(self):

        self.initialized = False
        self.node = rclpy.create_node('custom_subscriber')

    def listener_callback(self, msg):
        print('Received msg: "%s"' % msg.data)

    def create_subscriber(self, topicName):
        self.topicName = topicName
        self.subscription = self.node.create_subscription(
            String,
            self.topicName,
            self.listener_callback,
            10)
        self.initialized = True
        


class OgnROS2CustomPythonNode:
    """
         My custom ros2 python node
    """

    @staticmethod
    def internal_state():
        try:
            rclpy.init()

        except:
            pass
        return OgnROS2CustomPythonNodeInternalState()
    
    @staticmethod
    def compute(db) -> bool:
        """Compute the outputs from the current input"""

        state = db.internal_state
        if (not state.initialized):
            state.create_subscriber(db.inputs.topicName)
        
        rclpy.spin_once(state.node, timeout_sec=0.01)
        return True
    

    @staticmethod
    def release(node):
        try:
            state = OgnROS2CustomPythonNodeDatabase.per_node_internal_state(node)
        except Exception:
            state = None
            # print(Exception)
            pass

        if state is not None:

            state.node.destroy_node()
            try:
                rclpy.shutdown()
            except:
                pass

Notice that you need to import the autogenerated OgnROS2CustomPythonNodeDatabase module located in your extension which is used in release method to get the internal variables. The release method is called every time the omnigraph node is deleted or the stage is cleared. It does the necessary ros2 node cleanup required for the Isaac Sim ROS2 Bridge to function properly.

We are looking into turning this into a Isaac Sim Ros2 Bridge tutorial in the near future.
Hope this helps!