rosservice中的回调函数中调用其它线程中的数据作为返回值

本文链接：https://blog.csdn.net/qq_15204179/article/details/130723665

rosservice中的回调函数中调用其它线程中的数据作为返回值

在ROS中，rosservice的回调函数是在单独的线程中执行的，而在回调函数中直接调用其他线程中的数据作为返回值可能会导致问题，因为在多线程环境下，数据访问可能存在竞争条件和不确定性。

为了安全地在回调函数中访问其他线程中的数据并将其作为返回值，可以使用同步机制来确保数据的正确性。以下是一个可能的解决方案：

在回调函数中定义一个全局变量，用于保存要返回的数据。确保对该变量的访问是原子的，以防止竞争条件。在C++中，可以使用互斥锁来实现这一点。
在数据所在的线程中，提供一个接口或方法，用于安全地读取该数据并将其复制到全局变量中。在读取数据时，确保获取互斥锁，以确保同一时间只有一个线程访问数据。
在回调函数中，当需要返回数据时，获取互斥锁，并使用全局变量中的数据作为返回值。

下面是一个示例代码，演示了如何在回调函数中访问其他线程中的数据：

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

std::mutex data_mutex;
int shared_data = 0;

// 在数据所在的线程中读取数据并复制到全局变量中
void readDataThread()
{
    while (ros::ok())
    {
        std::lock_guard<std::mutex> lock(data_mutex);
        // 读取数据并更新 shared_data
        shared_data = readDataFromThread();
        // 延时或其他操作
        ros::Duration(1.0).sleep();
    }
}

// 服务回调函数
bool serviceCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
{
    std::lock_guard<std::mutex> lock(data_mutex);
    // 使用全局变量 shared_data 作为返回值
    res.data = shared_data;
    return true;
}

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

    // 创建服务
    ros::ServiceServer service = nh.advertiseService("example_service", serviceCallback);

    // 创建数据读取线程
    std::thread data_thread(readDataThread);

    ros::spin();

    // 等待数据读取线程退出
    data_thread.join();

    return 0;
}

在这个例子中，readDataThread函数在一个单独的线程中循环读取数据，并将其保存在全局变量shared_data中。serviceCallback函数作为rosservice的回调函数，在需要返回数据时，通过获取互斥锁，使用shared_data作为返回值。

请注意，在实际的应用中，你可能需要根据具体情况进行适当的修改和调整。确保在多线程环境中使用同步机制来保证数据的正确性和线

二.希望在serviceCallback函数执行后等待状态机执行完成，并将状态机的数据传递给serviceCallback的回调参数。这可以通过使用条件变量来实现。下面是修改后的代码示例：

#include <ros/ros.h>
#include <mutex>
#include <condition_variable>

std::mutex data_mutex;
std::condition_variable data_cv;
bool data_ready = false;

// 导航状态机类
class NavigationStateMachine {
    // 省略其他代码

public:
    // 增加成员函数用于通知数据准备完毕
    void notifyDataReady() {
        std::lock_guard<std::mutex> lock(data_mutex);
        data_ready = true;
        data_cv.notify_one();
    }
};

NavigationStateMachine state_machine_;

// 在数据所在的线程中读取数据并复制到全局变量中
void readDataThread()
{
    ros::Rate r(10);
    while (ros::ok()) {
        state_machine_.execute();
        ros::spinOnce();
        r.sleep();
         //数据准备完毕当前状态为monitor_progress这个时才能发起通知
        if(state_machine_.get_current_state()=="monitor_progress"){
         // 通知数据已准备完毕
       state_machine_.notifyDataReady();
      std::cout << " 2.thread_msg（） 通知数据已准备完毕  current_state:"<<state_machine_.get_current_state() << std::endl;

            }
    }
}

// 服务回调函数
bool serviceCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
{
    int state_params_size = state_machine_.set_state("navigate_to_goalPath", {start_pose, request.request_goal, target_pose_default_, std::string("EMPTY")});
    
    // 等待数据准备完毕
    std::unique_lock<std::mutex> lock(data_mutex);
    data_ready = false;  // Reset the flag before waiting.
    data_cv.wait(lock, []{ return data_ready; });
    
    // 获取状态机的数据
    boost::any age;
    if (state_machine_.get_current_state_param(0, age)) {
        // 执行你的操作，使用获取的数据
    }

    return true;
}

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

    // 创建服务
    ros::ServiceServer service = nh.advertiseService("example_service", serviceCallback);

    // 创建数据读取线程
    std::thread data_thread(readDataThread);

    ros::spin();

    // 等待数据读取线程退出
    data_thread.join();

    return 0;
}

在上述代码中，我们添加了一个条件变量data_cv和一个布尔变量data_ready来通知serviceCallback函数数据已经准备完毕。在readDataThread中，当数据准备完毕时，我们调用state_machine_.notifyDataReady()来通知等待的线程。

在serviceCallback函数中，我们使用std::condition_variable来等待数据的准备。我们使用data_cv.wait(lock, []{ return data_ready; })来等待条件变量，直到数据准备完毕。然后，我们可以使用state_machine_.get_current_state_param()来获取状态机的数据。

请注意，在实际应用中，你可能需要根据具体情况进行适当的修改和调整。确保在多线程环境中使用适当的同步机制来保证数据的正确性和线程间的协调。

这两个函数的运行原理如下：

notifyDataReady()函数：
- 获取data_mutex的独占锁lock_guard，确保在修改data_ready标志位时的线程安全性。
- 将data_ready标志位设置为true，表示数据已准备完毕。
- 调用data_cv.notify_one()，通知一个正在等待的线程，即data_cv.wait()被阻塞的线程，表示数据已准备完毕，可以继续执行。
data_cv.wait(lock, []{ return data_ready; })函数：
- 获取data_mutex的独占锁unique_lock，确保在等待条件变量时的线程安全性。
- 调用data_cv.wait()，将当前线程阻塞，直到满足以下条件之一：
  - data_ready标志位为true，即数据已准备完毕，或
  - 被其他线程调用notifyDataReady()通知。
- 在条件变量等待期间，unique_lock会自动释放锁，以允许其他线程访问共享资源，从而避免了忙等待。
- 当满足等待条件时，wait()函数会返回，并重新获取锁，线程可以继续执行后续代码。