如何在C++中通过POSIX和信号机使用ROS服务和串口通信?

2022-08-15 00:00:00 multithreading ros semaphore c++

我是C/C++中的并发和并行编程新手,所以我的项目需要一些帮助。

我希望在C++中使用POSIX和信号量运行多个进程。因此,程序的结构应该如下所示。 第一个I打开串口(Raspberry PI 4的串口通信)。在打开串口时,两个进程正在运行

第一个是自动运行的主进程,并执行以下操作: 该线程请求奥多姆更新(来自微控制器的压力和IMU)并发布它们。此外,每隔0.3秒检查一次调制解调器收件箱,如果有新消息发布。

另一个仅按需从ROS服务检测到调制解调器收件箱中有新消息,(在第一个主进程上)暂停,并在串口上执行(发布)。然后,第一个进程恢复正常工作

所以我试着先做一些伪C++代码,看起来像这些,但我需要帮助,因为我对并发性和并行性是新手。在这里

#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("
Completed...
"); //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);
}

所以我不确定这是在C++上实现的正确方式。在实现方面有什么帮助,或者在实际的C++代码方面也有帮助吗? 谢谢


解决方案

幸运的是,ROS允许您决定要使用的线程模型(http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning)。

您可以使用ros::AsyncSpinner

您的主线程启动在后台运行的AsyncSpner,侦听ROS消息,并在其自己的线程中调用ROS回调函数。

然后,您的主线程关心您的串口连接并转发/发布消息。在伪代码中,它可能如下所示:

#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
    }
}

您可以看到关键的代码块。在这里,两个线程是同步的,即一次只有一个线程在其关键路径中。

我使用的是C++互斥锁,因为它是C++的STD方式,但您当然可以更改它。

此外,请随时等待主线程中的串口消息,以减少芯片上的散热。

相关文章