C++和ROS的callback多线程运行

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Point.h>
#include <thread>
#include <mutex>
#include <condition_variable>

std::mutex mtx;
std::condition_variable cv;
bool follow_flag = false;
geometry_msgs::Point target_point;

void targetPointCallback(const geometry_msgs::Point::ConstPtr& msg) {
    std::lock_guard<std::mutex> lock(mtx);
    target_point = *msg;
    ROS_INFO("Received target point: x=%.2f, y=%.2f, z=%.2f", target_point.x, target_point.y, target_point.z);
    follow_flag = true;
    cv.notify_all();  // Notify follow thread
}

void followThread() {
    while (true) {
        std::unique_lock<std::mutex> lock(mtx);
        cv.wait(lock, []{ return follow_flag; });
        if (!follow_flag) {
            break;
        }
        ROS_INFO("Following target point: x=%.2f, y=%.2f, z=%.2f", target_point.x, target_point.y, target_point.z);
        // Implement your follow logic here
        // std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    ROS_INFO("Follow task ended.");
}

void followFlagCallback(const std_msgs::Bool::ConstPtr& msg) {
    std::lock_guard<std::mutex> lock(mtx);
    follow_flag = msg->data;
    if (follow_flag) {
        ROS_INFO("Start following.");
        cv.notify_all();  // Notify follow thread
    } else {
        ROS_INFO("Stop following.");
    }
}

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

    ros::Subscriber target_point_sub = nh.subscribe<geometry_msgs::Point>("target_point_topic", 1, targetPointCallback);
    ros::Subscriber follow_flag_sub = nh.subscribe<std_msgs::Bool>("follow_flag_topic", 1, followFlagCallback);

    std::thread follow_thread(followThread);

    ROS_INFO("Robot follow node initialized.");

    ros::spin();  // Spin ROS callbacks

    follow_thread.join();  // Wait for follow thread to finish

    return 0;
}