rclpy with ROS2 and two subscribers: second callback is not working

[anacm]

Hi everybody:

I need to work with CoppeliaSim and ROS2 using Python. Since the simROS2 option has some issues with Python, I am using rclpy. My goal now is to adapt the ControlledViaRos2.ttt scene from Lua to Python.

In that scene I have a problem with the two subscriber nodes that get the left motor and right motor velocities from the ros2BubbleRob ROS2 package. The first subscriber (left motor) gets the velocity ok, but the second one (right motor) does not get that velocity. I have checked that the ROS2 package is publishing the speed values on the left motor and right motor topics properly.

After some research, it seems that apparently the callback of the left motor subscriber is blocking the right motor callback. I do not know how to solve this: should I use a threaded script? Or maybe a Multi-Threaded executor could manage this (though from the documentation I don't see how to adapt it to the CoppeliaSim script structure)

I have enclosed my code below. I would really appreciate your help, I am at my wit's end... Thanks in advance,
Ana+

Code: Select all

import rclpy
from rclpy.node import Node

import math

from std_msgs.msg import Float32
from std_msgs.msg import Bool


def sysCall_init():
    sim = require('sim')
    
    print("[INIT] Getting handles of my world...")
    self.robotHandle=sim.getObject('..')
    self.leftMotor=sim.getObject("../LeftMotor") # Handle of the left motor
    self.rightMotor=sim.getObject("../RightMotor") # Handle of the right motor
    self.noseSensor=sim.getObject("../SensingNose") # Handle of the proximity sensor
    self.drawingCont=sim.addDrawingObject(sim.drawing_linestrip+sim.drawing_cyclic,0.005, 0, -1, 1000, [0, 1, 0])
    
    print("[INIT] Creating topic names...")
    leftMotorTopicName='leftMotorSpeed' 
    rightMotorTopicName='rightMotorSpeed' 
    sensorTopicName='sensorTrigger' 
    simulationTimeTopicName='simTime' 
    
    print("[INIT] Creating publishers and subscribers...")
    rclpy.init()
    self.sensorPub_node = rclpy.create_node('sensorPub')
    self.sensorPub_node_publisher = self.sensorPub_node.create_publisher(Bool,'/'+sensorTopicName, 10)
    
    self.simTimePub_node = rclpy.create_node('simTimePub')
    self.simTimePub_node_publisher = self.simTimePub_node.create_publisher(Float32,'/'+simulationTimeTopicName, 10)
    
    self.leftMotorSub_node = rclpy.create_node('leftMotorSub')
    self.leftMotorSub_node_subscriber = self.leftMotorSub_node.create_subscription(
       Float32,'/'+leftMotorTopicName, lambda msg: sim.setJointTargetVelocity(self.leftMotor,msg.data), 10)
    
    self.rightMotorSub_node = rclpy.create_node('rightMotorSub')
    self.rightMotorSub_node_subscriber = self.rightMotorSub_node.create_subscription(
       Float32,'/'+rightMotorTopicName, lambda msg: sim.setJointTargetVelocity(self.rightMotor,msg.data), 10)
    
    result=sim.launchExecutable('ros2BubbleRob',leftMotorTopicName+" "+rightMotorTopicName+" "+sensorTopicName+" "+simulationTimeTopicName,0)
    

def sysCall_sensing():
    print("[SENSING] BubbleRob is moving..")
    p=sim.getObjectPosition(self.robotHandle)
    sim.addDrawingObjectItem(self.drawingCont,p)
    

def sysCall_actuation():
    # Send an updated sensor and simulation time message, and send the transform of the robot:
    result=sim.readProximitySensor(self.noseSensor)
    print("[ACTUATION] Proximity sensor says...",bool(result[0]))
    detectionTrigger = Bool()
    detectionTrigger.data = bool(result[0])
    currentSimTime = Float32()
    currentSimTime.data = sim.getSimulationTime()
    print("[ACTUATION] Publishing in topics...")
    self.sensorPub_node_publisher.publish(detectionTrigger)
    self.simTimePub_node_publisher.publish(currentSimTime)
    print("[ACTUATION] Reading left motor subscription...",sim.getJointTargetVelocity(self.leftMotor))
    print("[ACTUATION] Reading right motor subscription...",sim.getJointTargetVelocity(self.rightMotor))
    # Send the robot's transform:
    #simROS2.sendTransform(getTransformStamped(robotHandle,'ros2InterfaceControlledBubbleRob',-1,'world'))
    print("[ACTUATION] Everybody is spinning...")
    rclpy.spin_once(self.sensorPub_node, timeout_sec=0)
    rclpy.spin_once(self.simTimePub_node, timeout_sec=0)
    rclpy.spin_once(self.leftMotorSub_node, timeout_sec=0)
    rclpy.spin_once(self.rightMotorSub_node, timeout_sec=0)
    
    
def sysCall_cleanup():
    print("[CLEANUP] Cleaning...")
    self.sensorPub_node.destroy_node()
    self.simTimePub_node.destroy_node()
    self.leftMotorSub_node.destroy_node()
    self.rightMotorSub_node.destroy_node()
    rclpy.shutdown()

def getTransformStamped(objHandle,name,relTo,relToName):
    t=sim.getSystemTime()
    p=sim.getObjectPosition(objHandle,relTo)
    o=sim.getObjectQuaternion(objHandle,relTo)
    return {
        'header': {
            'stamp': t,
            'frame_id': relToName
        },
        'child_frame_id': name,
        'transform': {
            'translation': {'x': p[0], 'y': p[1], 'z': p[2]},
            'rotation': {'x': o[0], 'y': o[1], 'z': o[2], 'w': o[3]}
        }
    }

[fferri]

Why are you creating 4 nodes?

rclpy.spin_once() processes callbacks one node at a time, in separate rclpy Node executors.
With one callback doing synchronous work inside the CoppeliaSim thread, it can block the others until its executor returns.

Instead of 4 nodes, create just one rclpy node with multiple subscribers, and one executor call only.

[anacm]

I was trying to translate the "controlledViaRos2" scene, and I thought that every publisher or subscriber should be an independent node.

Now I see I was wrong. I have modified the "ros2InterfaceTopicPublisherAndSubscriber" scene so a node manages all the publishers and other node manages all the subscription, each one is spun once and it works fine.

Thanks a lot for your help :)!!