ROS多线程节点共享变量阻塞问题求助及代码优化方案

问题原因 #

• 共享变量未被正确保护：get_value()中直接读取batch_size的while循环未加锁，编译器可能对该变量做寄存器缓存优化，导致计算线程无法感知订阅线程对batch_size的更新，陷入无限循环。同时计算平均值时读取sum_of_error和batch_size也未加锁，会出现数据不一致的竞态问题。
• 缺乏线程同步机制：仅用互斥锁保护变量修改，但未使用条件变量实现“等待batch_size达标”的同步逻辑，忙等循环不仅浪费CPU资源，也无法保证线程间事件通知的及时性。
• 重置逻辑时序错误：计算平均值的操作在加锁重置之前，此时sum_of_error和batch_size可能被订阅线程同时修改，导致计算出的平均值不准确，甚至重置后订阅线程的修改被覆盖。

解决方案 #

使用std::condition_variable实现线程间同步，确保所有访问共享变量（batch_size、sum_of_error）的操作都在互斥锁保护下进行，替换忙等循环为条件等待：

修改后的完整代码 #

#include <ros/ros.h>
#include <string>
#include <vector>
#include <std_msgs/Float32.h>
#include <dynamic_reconfigure/client.h>
#include <calibration/HcNodeConfig.h>
#include <mutex>
#include <thread>
#include <condition_variable>

using namespace std;

mutex m;
condition_variable cv;
int batch_size = 0;
float sum_of_error = 0;
int BATCH = 10;

float get_value() {
    unique_lock<mutex> lock(m);
    // 等待batch_size达标，同时处理虚假唤醒
    cv.wait(lock, []{ return batch_size >= BATCH; });
    
    float res = sum_of_error / batch_size;
    // 重置共享变量
    sum_of_error = 0.0;
    batch_size = 0;
    lock.unlock();
    return res;
}

void adjuster_callback(const hc::HcNodeConfig& data) {
    // 按需实现动态重配置回调逻辑
}

void listener_callback(const std_msgs::Float32ConstPtr& msg) {
    float current_error = msg->data;
    lock_guard<mutex> lock(m);
    batch_size++;
    sum_of_error += current_error;
    
    // 当batch_size达标时，通知等待的计算线程
    if (batch_size >= BATCH) {
        cv.notify_one();
    }
}

void start_listener(ros::NodeHandle n) {
    ros::Subscriber listener = n.subscribe("/error", BATCH, listener_callback);
    ros::Rate loop_rate(BATCH);
    while (ros::ok()) {
        ros::spinOnce();
        loop_rate.sleep();
    }
}

void start_adjuster(ros::NodeHandle n) {
    ros::Rate loop_rate(BATCH);
    hc::HcNodeConfig config;
    dynamic_reconfigure::Client<hc::HcNodeConfig> client("/lidar_front_left_hc", adjuster_callback);
    ros::Duration d(2);
    client.getDefaultConfiguration(config, d);
    
    while (ros::ok()) {
        float current_error = get_value();
        if (current_error > 0.0) {
             config.angle = 0.0;
             client.setConfiguration(config);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "adjuster");
    ros::NodeHandle n;

    thread t1(start_listener, n);
    thread t2(start_adjuster, n);

    t1.join();
    t2.join();
    return 0;
}

关键修改点 #

• 添加std::condition_variable cv用于线程间的同步通知。
• get_value()中使用unique_lock配合cv.wait()等待条件满足，避免忙等，wait()会自动释放锁，被唤醒后重新获取锁，确保访问共享变量的线程安全。
• listener_callback()在更新共享变量后，检查batch_size是否达标，若达标则调用cv.notify_one()唤醒等待的计算线程。
• 所有访问batch_size和sum_of_error的操作都被互斥锁保护，彻底消除竞态条件和编译器缓存问题。

内容的提问来源于stack exchange，提问作者errorcodemonkey