Robotics StackExchange | Archived questions

ValueError while looping spin_once() in a separate thread from a custom pyqtgraph widget

I am trying to write a subscriber node within a custom PyQt5 widget (via the module pyqtgraph). The widget is simple: a dot rotating in xy space, subscribing to a Twist message as a command velocity.

Because I am using pyqtgraph, my python process is run by the Qt event loop (app.exec(), called by pyqtgraph as pg.exec()). This prevents me from using rclpy.spin() to manage the subscriber node, so I am using rclpy.spin_once() on a timed loop clocked by the Qt timer. This also forces my program to be structured like a PyQt application where my ROS node is an attribute of the MainWindow object, as opposed to defining a subscriber class that inherits rclpy.Node. I understand this might not be the best practice, but the features and performance provided by the Qt event loop are important to my full application, for which the rotating dot is just a temporary stand-in.

If I run my program as described above, the plotting is choppy, as Qt needs to wait for spin_once to complete. To achieve smoother plotting, I decided to run all calls to spin_once in a separate thread operated by PyQt's QRunnable class. As a result, I am making frequent (~50 Hz) calls to spin_once in a thread pool run by PyQt. When I run it this way, I get the following runtime error:

File "/opt/ros/humble/.../rclpy/executors.py", line 690, in wait_for_ready_callbacks
    return next(self._cb_iter)
ValueError: generator already executing

I assume this error comes from the subscriber node being called more frequently than it can handle, but I also know that 50 Hz is not an unreasonable rate for topic subscription. How could I structure this program so that I can run the Qt app.exec() and also subscribe to a topic? Is there a better way to run repeated calls to a subscriber in a separate thread?

I am still a novice to ROS and concepts like threading and multiprocessing, but I'm open to rewriting this in a different structure so long as I am still able to use my pyqtgraph GUI widget.

I am running ros2 Humble on Ubuntu Jammy (22.04) on an x86 Dell XPS 13, with kernel 5.15.0-46-generic. My full program is written below:

import rclpy
import pyqtgraph as pg
from geometry_msgs.msg import Twist

from PyQt5 import QtWidgets, QtCore
from PyQt5.QtCore import QRunnable, QThreadPool, pyqtSlot
from rclpy.node import Node

class Worker(QRunnable):
    '''
    This is a utility for running functions in a thread outside the Qt event loop
    '''
    def __init__(self, fn, *args, **kwargs):
        super(Worker, self).__init__()
        # Store constructor arguments (re-used for processing)
        self.fn = fn
        self.args = args
        self.kwargs = kwargs

    @pyqtSlot()
    def run(self):
        # Initialise the runner function with passed args, kwargs.
        self.fn(*self.args, **self.kwargs)

class MainWindow(pg.GraphicsLayoutWidget):
    '''
    This is a pyqtgraph widget that plots a circular dot in xy space.
    '''
    def __init__(self,parent=None, show=True, size=None, title=None, **kargs):
        super().__init__(parent, show, size, title, **kargs)

        #Initialize ROS and subscriber node
        rclpy.init(args=None)
        self.node = Node("Plotter")
        self.sub = self.node.create_subscription(Twist, 'cmd_vel', self.get_msg, 10)

        #initialize Qt thread pool
        self.threadpool = QThreadPool()

        #rate and timer
        self.timer_period = 20 #ms
        self.timer = QtCore.QTimer()
        self.timer.setTimerType(QtCore.Qt.PreciseTimer)
        self.timer.timeout.connect(self.timer_update)   

        #window config
        self.setGeometry(0, 0, 400,400)

        #set background to white
        self.setBackground('w')

        # initialize plot 
        self.P = self.addPlot()
        self.P.setRange(xRange=(-1.2,1.2),yRange=(-1.2,1.2))
        self.P.setAspectLocked(lock=True, ratio=1)

        # initialize positions and velocities
        self.x = 1
        self.y = 0
        self.dx = 0
        self.dy = 0
        self.do = 0

        #initialize a dot
        self.dot = self.P.plot([self.x],[self.y],pen=None,symbolBrush='k',symbolPen='w',symbolSize=50)

        # start timer
        self.timer.start(self.timer_period)

    def timer_update(self):
        '''
        This function goes off every (self.timer_period) milliseconds and spins the ros node to
        communicate with the ros graph and update things.
        '''
        #spin ros node (in a separate thread)
        worker = Worker(rclpy.spin_once,self.node) 
        self.threadpool.start(worker)

        #update the plot 
        self.update_plot()

    def get_msg(self,msg):
        '''
        Fetch information from the Twist topic
        '''
        #unpack Twist message
        self.dx = msg.linear.x
        self.dy = msg.linear.y
        self.do = msg.angular.z

    def update_plot(self):
        '''
        Update the position of the dot, setting data to the plot
        '''
        #update the dot
        self.x = self.x + self.dx
        self.y = self.y + self.dy

        #set data
        self.dot.setData(x=[self.x],y=[self.y])

        #print out the current values
        print('*****')
        print('x = ',self.x)
        print('y = ',self.y)

        # show
        self.show()

if __name__ == '__main__':
    app = QtWidgets.QApplication([])
    window = MainWindow()
    pg.exec()

Thank you!

Asked by markg on 2022-08-29 17:08:25 UTC

Comments

It will depend a bit on what you want/need to do exactly, but I've found StefanFabian/qml_ros2_plugin to be really convenient for creating UIs that visualise data coming in over ROS topics.

Asked by gvdhoorn on 2022-09-01 01:19:59 UTC

I'll check this out, thank you!

Asked by markg on 2022-09-01 10:38:41 UTC

Answers

I found a solution! My mistake here was trying to use a thread-based solution when I could instead just use multiple processes and inter-process communication provided by QProcess. If anyone has a simple PyQt GUI they want to communicate with ROS, this is a fairly simple (maybe trivial?) solution that allows you to run both rclpy.spin and a PyQt app at the same time, though in different processes.

My code is made up of two scripts: plotter.py and subscriber.py, where plotter.py is the pyqtgraph widget and subscriber.py is the ROS node. To launch both programs, run plotter.py and it will create a new process via QProcess that will launch subscriber.py. The node enters a rclpy.spin loop and outputs a copy of the current message reading to its standard output, only visible by its parent process plotter.py.

I hacked this together very quickly and am fairly new to this so please let me know if there are any obvious improvements to be made. I am unsure whether standard output is the best method for IPC in this case, but it's fast enough for my application.

Here is my code for the ROS node, in subscriber.py

'''
This is a ROS2 subscriber that subscribes to the /cmd_vel topic, and then
"re-publishes" that information to standard output, which is readable by QProcess.
'''

import rclpy
import sys
from rclpy.node import Node
from geometry_msgs.msg import Twist

class QtSubscriber(Node):
    def __init__(self):
        super().__init__('sub_to_qt')

        #init subscription
        self.subs = self.create_subscription(Twist,'cmd_vel',self.callback,10)
        self.subs

    def callback(self,msg):
        # get data from the Twist message
        self.x  = msg.linear.x
        self.y  = msg.linear.y
        self.th = msg.angular.z

        #output
        # NOTE: these messages will be parsed such that the number on the
        # right-hand side of the '=' is read, and if there is no '=', the 
        # whole message is read. So: either make sure everything to the 
        # right of '=' is numeric or make the whole string numeric.
        sys.stdout.write(f"x  = {self.x}\n")
        sys.stdout.write(f"y  = {self.y}\n")
        sys.stdout.write(f"th = {self.th}\n")
        sys.stdout.flush()

def main(args=None):
    rclpy.init(args=args)
    node = QtSubscriber()

    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Here is my code for the pyqtgraph widget, in plotter.py

'''
This script aims to create a pyqtgraph app which also acts as a ros node. 
Timing will be run by the Qt event loop, which should be run at a faster 
rate than ros publishes data at to prevent data loss.
'''
import sys
import pyqtgraph as pg

from PyQt5 import QtWidgets, QtCore
from PyQt5.QtCore import QProcess

from IPython import embed


class MainWindow(pg.GraphicsLayoutWidget):
    '''
    This is a pyqtgraph widget that plots a circular dot in xy space.
    '''
    def __init__(self,parent=None, show=True, size=None, title=None, **kargs):
        super().__init__(parent, show, size, title, **kargs)

        #Initialize ROS subscriber process
        self.p = None
        self.initROSproc()

        #rate and timer
        self.timer_period = 20 #ms
        self.timer = QtCore.QTimer()
        self.timer.setTimerType(QtCore.Qt.PreciseTimer)
        self.timer.timeout.connect(self.update_plot)   

        #window config
        self.title = 'blarf'
        self.width = 400
        self.height = 400
        self.setWindowTitle(self.title)
        self.setGeometry(0, 0, self.width, self.height)
        #set background to white
        self.setBackground('w')

        # initialize plot
        self.P = self.addPlot()
        self.P.setRange(xRange=(-1.2,1.2),yRange=(-1.2,1.2))
        self.P.setAspectLocked(lock=True, ratio=1)

        # initialize positions and velocities
        self.x = 1
        self.y = 0
        self.dx = 0
        self.dy = 0
        self.do = 0

        #initialize a dot
        self.dot = self.P.plot([self.x],[self.y],pen=None,symbolBrush='k',symbolPen='w',symbolSize=50)

        #start timer
        self.timer.start(self.timer_period)   

    def update_plot(self):
        '''
        Update the position of the dot, setting data to the plot
        '''
        #update the dot
        self.x = self.x + self.dx
        self.y = self.y + self.dy

        #set data
        self.dot.setData(x=[self.x],y=[self.y])

        #print out the current values
        print('*****')
        print('x = ',self.x)
        print('y = ',self.y)

        # show
        self.show()

    def initROSproc(self):
        '''
        This initializes a separate python process (subscriber.py) and sets up communication with that process
        '''
        if self.p is None:  # No process running.
            #initialize process
            self.p = QProcess()  # Keep a reference to the QProcess (e.g. on self) while it's running.
            self.p.readyReadStandardOutput.connect(self.get_msg)
            self.p.finished.connect(self.process_finished)  # Clean up once complete.

            #start process
            print('starting ROS process')
            self.p.start("python3", ['subscriber.py'])

    def process_finished(self):
        print("Process finished.")
        self.p = None

    def get_msg(self):
        '''
        Fetch information from the standard ouput
        '''
        #get standard output string
        data = self.p.readAllStandardOutput()
        stdout = bytes(data).decode("utf8")

        #parse standard input and assign values
        parse_xyth = [float(x.split("=")[-1]) for x in stdout.split("\n")[0:3]]

        #unpack Twist message
        self.dx = parse_xyth[0]
        self.dy = parse_xyth[1]
        self.do = parse_xyth[2]

def main():
    app = QtWidgets.QApplication([])
    window = MainWindow()
    pg.exec()


if __name__ == '__main__':
    main()

Asked by markg on 2022-08-31 10:16:39 UTC

Comments