How to use ROS services and serial comunication with posix and semahrores in C++?

Question

Im new to Concurrency and parallelism programming in C/C++ so I need some help woth my project.

I want to run multiple process using POSIX and Semaphores in C++. 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 C++ code that looks like these But I need help as Im new to Concurrency and parallelism. Here it is

#include <stdio.h>
#include <pthread.h>
#include <semaphore.h>
#include <unistd.h>
sem_t mutex;

void* thread(void* arg) { //function which act like thread

   //Main Thread
   // Here code for ASK ODOM UPDATE..
   // Here code for CHECK MODEM INBOX...
   sem_wait(&mutex); //wait state
   
   // ENTER in the second Process
   // Here code for the second process which run on DEMAND..
   // ROS SERVICES
   
   // Here code for CHECK The MODEM INBOX and HALT the First Process
   // Here code for EXECUTE on SERIAL PORT(PUBLISH)
   sleep(0.1); //critical section 
   printf("\nCompleted...\n"); //comming out from Critical section
   sem_post(&mutex);
}

main() {
   sem_init(&mutex, 0, 1);
   pthread_t th1,th2;
   pthread_create(&th1,NULL,thread,NULL);
   sleep(1);
   pthread_create(&th2,NULL,thread,NULL);
   //Join threads with the main thread
   pthread_join(th1,NULL);
   pthread_join(th2,NULL);
   sem_destroy(&mutex);
}

So Im not sure this is the correct way of implementation on C++ . Any help in the implementation and maybe help on the actual C++ code? Thanks

Answer 1

Fortunately, ROS lets you decide what threading model you want to use (http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning).

You could use the ros::AsyncSpinner for that:

Your main thread starts the AsyncSpinner that runs in the background, listens to ROS messages, and calls your ROS callback functions in its own threads.

Then, your main thread cares about your serial port connection and forwards/publishes messages. In pseudo-code, it could look as follows:

#include <ros/ros.h>
#include <mutex>
#include <std_msgs/Float32.h>

std::mutex mutex_;

void callback(std_msgs::Float32ConstPtr msg) {
    // reentrant preprocessing
    {
        std::lock_guard<std::mutex> guard( mutex_ );
        // work with serial port
    }
    // reentrant posprocessing
}

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "name");
    ros::NodeHandle node("~");
    ros::Subscriber sub = node.subscribe("/test", 1, callback);

    ros::AsyncSpinner spinner(1);
    spinner.start();

    while(ros::ok()) {
        // reentrant preprocessing
        {
            std::lock_guard<std::mutex> guard(mutex_);
            // work with serial port
        }
        // reentrant postprocessing
    }
}

You can see the code block that is critical. Here, both threads are synchronized, i.e., only one thread is in its critical path at a time.

I used the C++ mutex, as it is the C++ std way, but you could change that of course.

Also, feel free to wait for serial port messages in the main thread to reduce heat production on your chip.