Search code examples
pythonmultithreadingros

How to use "multi-threading" in python with ROS services and serial comunication?

I want to run multiple process in python with ROS services and serial communication (between Raspberry Pi and ESP32 Microcontroller) in my robot. The software schematic in the robot is shown in this imageSoftware schematic on the robot

I start doing in C++ as can see from this link How to use ROS services and serial communication with posix and semaphores in C++? but I realized that my modules for serial communication and the ODOM(Pressure, IMU) read are in python.

So, the structure of the program should be the following one. First I Open Serial port (Serial communication of the Raspberry PI 4). While the Serial is Open Two processes are running

First one, the main one run automatically and do the following: The thread ask for ODOM Updates(Pressure and IMU from the microcontroller) and Publish them. Also every 0.3 seconds check the modem inbox and if something new it publish.

The other one only on DEMAND from ROS Services detect that there is new message in the modem inbox do HALT( on the first main Process) and execute (publish) on the serial Port. Then the first process resume with normal work

So I trying to do first some pseudo python code that looks like these But I need help as Im new to python and multithreading. Here it is

#!/usr/bin/env python3

from sys import ps1
import rospy
import numpy as np
from os import system
import time
import threading
import Microcontroller_Manager_Serial as Serial
import IMU_Functions as IMU
import Pressure_Functions as Pressure
from std_msgs.msg import Float32

mutex = threading.Lock()

from std_msgs.msg import String
Communication_Mode_ = 0 # Serial Communication

pub_pressure = rospy.Publisher('depth',Float32,queue_size=1)

P0 = 1.01325 #Default Pressure


def callback(data):
    global P0
    mutex.acquire()
    while (Serial.Serial_Port_Standard()): # While serial Port is open
        
        try:
            data_received_pressure = Pressure.Pressure_Get_Final_Values(1,1)
            data_received_imu = IMU.IMU_Get_Values(1, 1)
            P1 = (np.int16((data_received_pressure[6]<<24) | (data_received_pressure[7]<<16) | (data_received_pressure[8]<<8) | (data_received_pressure[9])))/10000
            P0 = (np.int16((data_received_pressure[6]<<24) | (data_received_pressure[7]<<16) | (data_received_pressure[8]<<8) | (data_received_pressure[9])))/10000

            P = P1 - P0 # Relative Measured Pressure
            pressure = P
            pub_pressure.publish(pressure)
         except:
             print ("pressure not obtained")

        Modem.Send_Data(Communication_Mode, receiver_ID, data_size, data_to_send)

def listener():

# Not sure how to check every 0.3 seconds the modem inbox and if something new it publish. And also the  DEMAND from ROS Services detect that there is new message in the modem inbox do HALT( on the first main Process) and execute (publish) on the serial Port. Then the first process resume with normal work

For the modem check up I though need something like this , but im not sure.

def callback_modem(data_in):

        data_in = Serial.Serial_Port_Receive_Data(20,0.2) 
        if (data_in[0] == 91) # Received data from acoustic modem
        rt = RepeatedTimer(1, data_in, "Recieved Data") # it auto-starts, no need of rt.start()
        modem_data= data_in
        pub_modem.publish(modem_data)

        try:
            sleep(0.3) # your long-running job goes here...
        finally:
            rt.stop() # better in a try/finally block to make sure the program ends!

The third thread with ROS Services callback should be like this?

def handle_ros_service(req):
    data_received_temperature = TEMPERATURE.TEMPERATURE_Get_Values(1, 1)
    Tempetrature = (np.int16((data_received_imu[6]<<24) | (data_received_imu[7]<<16) | (data_received_imu[8]<<8) | (data_received_imu[9])))/10000
    mutex.acquire(blocking=True)
    return handle_ros_service(req.Tempetrature)

 
def publish_on_serial():
    # send temperature over serial port
    mutex.release()
Solution

  • You could build a default, simple node with two callbacks, one of which is your message callback for the subscribed ROS topic, and one of which is a callback to a TimerEvent which can be called repeatedly every 0.3 seconds. You need to start the TimerEvent before rospy.spin() in the main.

    The code could look like this:

    #!/usr/bin/env python

import threading
import rospy
from std_msgs.msg import String

mutex = threading.Lock()


def msg_callback(msg):
    # reentrang processing
    mutex.acquire(blocking=True)
    # work serial port here, e.g. send msg to serial port
    mutex.release()
    # reentrant processing

def timer_callback(event):
    # reentrant processing
    mutex.acquire(blocking=True)
    # work serial port here, e.g. check for incoming data
    mutex.release()
    # reentrant processing, e.g. publish the data from serial port

def service_callback(req):
    # read out temperature
    mutex.acquire(blocking=True)
    # send temperature over serial port
    mutex.release()

    
if __name__ == '__main__':
    # initialize serial port here
    rospy.init_node('name')
    rospy.Subscriber('/test_in', String, msg_callback)
    rospy.Service('on_req', Empty, service_callback)
    rospy.Timer(rospy.Duration(0.3), timer_callback)
    rospy.spin()

    In fact, rospy starts two different threads for the callbacks. However, these threads are not running in parallel on different cores due to the GIL but they get scheduled and switch execution at certain system calls, e. g., when calling IO operations. Hence, you still need to sync your threads with a Lock as you did in your question.