0

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()
Macedon971
  • 303
  • 2
  • 11

1 Answers1

1

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.

J.P.S.
  • 485
  • 4
  • 11
  • The other one only on DEMAND from ROS Services im not sure how to implement it in python. Any help on that? – Macedon971 Mar 09 '22 at 07:18
  • I included a code example in the answer. Does that help? – J.P.S. Mar 09 '22 at 11:21
  • sorry but where is the process that on demand call ROS Service and do HALT for the first process? – Macedon971 Mar 09 '22 at 11:46
  • What exactly do you mean by *cal ROS Service*? Do you mean to publish a message, or to call another service topic? Anyhow, you could include that whereever necessary, e.g., right after reading something from the serial port in the timer_callback. By DEMAND, do you mean *upon arrival of a ROS message*? The would be the subscriber and the msg_callback. Also, by HALT, do you mean to pause the other thread? That would be solved by the mutex. – J.P.S. Mar 09 '22 at 13:29
  • So better to explain. I have first thread: ask for Odometry update( Pressure, IMU) and publish that topic , then every 0.3 seconds check the modem inbox. Then I have second thread that runs in parallel in background : By DEMAND of ROS SERVICE check lets say Temperature pause the first thread (HALT) and publish the Temperature on serial port. So I have 2 threads , first one is running (with odomerty topics and checking modem inbox) and second one (with ROS SERVICE) only ON DEMAND pause the first one and then let it continue running .Hope is clear now – Macedon971 Mar 10 '22 at 06:55
  • OK, sorry. So you really meant a ros service as an RPC (like [here](http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28python%29)). Well, than you simply add the ros service (which runs as a third thread) and again lock the serial port with the mutex in the service callback. – J.P.S. Mar 10 '22 at 07:42
  • why as third thread? Though only two threads. First with Odometry and modem and Second with the ROS Services. – Macedon971 Mar 10 '22 at 07:59
  • Because subscribing to a topic automatically opens a new thread in rospy. Since you have to leave the callback function to get more msgs on that topic, you can't ask for modem updates every 0.3 seconds on that thread. Hence, the regular updates need a second thread as well. Now, ros services also automaticall open a new thread, which is why you got the third one. – J.P.S. Mar 10 '22 at 08:09
  • ok. So how would be the python code with 3 threads? can you update in the answers? – Macedon971 Mar 10 '22 at 08:11
  • Sure, just look ahead, I already did that ;) – J.P.S. Mar 10 '22 at 08:12
  • ok. I checked . For the modem check up I though need something like I edit in the question , but im not sure. Why you suggest different way? – Macedon971 Mar 10 '22 at 08:19
  • your `callback_modem` does not make sense to me. For one, it gets a function parameter that you overwrite immediately. Furthermore, I don't know the API you are using. Eventually, it seems to me like kind of a "main thread". However, you have to give up the main thread as you need to call `rospy.spin()`. You could either start a new thread before that, which runs your `callback_modem`. Or you could go the ROS way and instead use the TimerEvent as I proposed. This would make the node more consistent regarding the ROS way to solve things. – J.P.S. Mar 10 '22 at 08:26
  • Ok. Yes I want to have a "main thread" that runs automatically ( the first two threds, the Odometry and Modem). And the third one (ROS Services) is only on DEMAND. So what should I do in that case? The code still remains same as your last one proposed? – Macedon971 Mar 10 '22 at 08:28
  • Well, that's the solution i put in my answer. – J.P.S. Mar 10 '22 at 08:32
  • ok. So sorry still not clear about ROS Services thread . When I look in ROS example pyhton the server node has two callbacks like this def handle_add_two_ints(req): print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))) return AddTwoIntsResponse(req.a + req.b) def add_two_ints_server(): rospy.init_node('add_two_ints_server') s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints) print("Ready to add two ints.") rospy.spin() – Macedon971 Mar 10 '22 at 08:36
  • So how to put that in one callback in my case? – Macedon971 Mar 10 '22 at 08:37
  • You got the tutorial wrong: the second function calls `rospy.spin()`. It is not a callback function but just called from the main routine. – J.P.S. Mar 10 '22 at 09:08
  • so in the first function will read the temperature data and do mutex.acquire(blocking=True) then in second will send temperature over serial port and mutex.release(). Correct? – Macedon971 Mar 10 '22 at 09:13
  • If you regard to the comments in the service_callback, then yes. Just replace the comments with your actual code that reads out temperature and sends it via serial port. – J.P.S. Mar 10 '22 at 10:36
  • I add the code for the third thread in the question, Is that correct? – Macedon971 Mar 10 '22 at 10:45