ROS MELODIC 多線程 發佈與訂閱

ROS  MELODIC  多線程 發佈與訂閱

這裏我只把代碼貼出來，裏面使用的boost::bind函數在傳指針或引用參數時，會傳不過去，試了很多遍了，網上查了很多資料也不行，這裏使用bind函數把回調函數傳給 node.advertise  或者node.subscribe 函數時，會有問題，會報no match 錯誤，個人C++也學得不很很好，所以這裏只用試試的心態，勿噴。。這裏只貼代碼，沒有什麼註釋，代碼可以運行，CMakeFile.txt 我就不加了，僅供參考。。

dbh_thread.h

#ifndef DBH_THREAD_H__
#define DBH_THREAD_H__

#include <ros/ros.h>
#include <ros/forwards.h>
#include <boost/thread.hpp>
#include <sstream>

#include <std_msgs/UInt16.h>
#include <std_msgs/String.h>

#define TP_SET_L_SPD "mtr_set_L_speed"        // 設置電機左右輪速度
#define TP_SET_R_SPD "mtr_set_R_speed"

#define TP_SET_EN "mtr_set_en"            // 電機使能
#define TP_SET_IO "mtr_set_io"            // 電機IO
#define TP_CLR_ALM "mtr_clr_alm"          // 清除電機警告

#define TP_GET_ENC_L "mtr_get_L_enc"      // 獲取左右輪編碼器
#define TP_GET_ENC_R "mtr_get_R_enc"

#define TP_GET_IO "mtr_get_io"
#define TP_GET_BAT "mtr_get_batt"        // 電池
#define TP_GET_ALM "mtr_get_alm"

#define TIME_DUR_1HZ    1
#define TIME_DUR_5HZ    0.2
#define TIME_DUR_10HZ   0.1
#define TIME_DUR_30HZ   0.03

class dbh_thread{
public:
    // 實始多多個進程
    dbh_thread(){
        sub_L_spd = nh.subscribe(TP_SET_L_SPD, 50, &dbh_thread::set_L_speed_cb, this);
        sub_R_spd = nh.subscribe(TP_SET_R_SPD, 50, &dbh_thread::set_R_speed_cb, this);
        sub_clr_alm = nh.subscribe(TP_CLR_ALM, 50, &dbh_thread::clr_alm_cb, this);
        sub_set_io = nh.subscribe(TP_SET_IO, 50, &dbh_thread::set_io_cb, this);
        sub_set_en = nh.subscribe(TP_SET_EN, 50, &dbh_thread::set_en_brk_cb, this);

        // 發佈者
        pub_L_enc = nh.advertise<std_msgs::UInt16>(TP_GET_ENC_L, 1000, 
                            boost::bind(&dbh_thread::L_enc_conn, this, _1 ),
                            boost::bind(&dbh_thread::L_enc_discon, this, _1)  );
        pub_R_enc = nh.advertise<std_msgs::UInt16>(TP_GET_ENC_R, 1000, 
                            boost::bind(&dbh_thread::R_enc_conn, this, _1), 
                            boost::bind(&dbh_thread::R_enc_discon, this,  _1 )  );
        pub_get_batt = nh.advertise<std_msgs::UInt16>(TP_GET_BAT, 1000, 
                            boost::bind(&dbh_thread::get_batt_conn,this, _1 ), 
                            boost::bind(&dbh_thread::get_batt_discon,this,  _1 )  );
        pub_get_io = nh.advertise<std_msgs::UInt16>(TP_GET_IO, 1000, 
                            boost::bind(&dbh_thread::get_io_conn,this, _1 ), 
                            boost::bind(&dbh_thread::get_io_discon,  this, _1 )  );
        pub_get_alm = nh.advertise<std_msgs::UInt16>(TP_GET_ALM, 1000, 
                            boost::bind(&dbh_thread::get_alm_conn,this, _1 ), 
                            boost::bind(&dbh_thread::get_alm_discon, this,  _1)  );
        
        //  初始化定器，定時發佈消息
        tm_L_enc = nh.createTimer(ros::Duration(TIME_DUR_1HZ),                              //  TIME_DUR_30HZ
                            boost::bind(&dbh_thread::L_enc_tm_cb,this, _1 ), false, false);
        tm_R_enc = nh.createTimer(ros::Duration(TIME_DUR_1HZ),                              //  TIME_DUR_30HZ
                            boost::bind(&dbh_thread::R_enc_tm_cb,this, _1 ), false, false);
        tm_get_batt = nh.createTimer(ros::Duration(TIME_DUR_1HZ),                          //  TIME_DUR_1HZ
                            boost::bind(&dbh_thread::get_batt_tm_cb,this, _1 ), false, false);
        tm_get_io = nh.createTimer(ros::Duration(TIME_DUR_1HZ),                              //  TIME_DUR_10HZ
                            boost::bind(&dbh_thread::get_io_tm_cb,this, _1 ), false, false);
        tm_get_alm = nh.createTimer(ros::Duration(TIME_DUR_1HZ),                           //  TIME_DUR_10HZ
                            boost::bind(&dbh_thread::get_alm_tm_cb,this, _1 ), false, false);                                              
    }

    //  發佈者回調
    void set_L_speed_cb(const std_msgs::UInt16 &msg);
    void set_R_speed_cb(const std_msgs::UInt16 &msg);
    void set_en_brk_cb(const std_msgs::UInt16 &msg);
    void set_io_cb(const std_msgs::UInt16 &msg);
    void clr_alm_cb(const std_msgs::UInt16 &msg);

    void L_enc_tm_cb(const ros::TimerEvent &tmEvt);
    void L_enc_conn(const ros::SingleSubscriberPublisher &pub);
    void L_enc_discon(const ros::SingleSubscriberPublisher &pub);

    void R_enc_tm_cb(const ros::TimerEvent &tmEvt);        // 定時器加回調
    // 有訂閱者與發佈者連接時的回調, 回調裏面會打開定時器，定時發佈消息
    void R_enc_conn(const ros::SingleSubscriberPublisher &pub);    
    // 有訂閱者與發佈者斷開連接時的回調, 回調裏面會暫時定時器，暫停定時發佈消息
    void R_enc_discon(const ros::SingleSubscriberPublisher &pub);

    void get_batt_tm_cb(const ros::TimerEvent &tmEvt);
    void get_batt_conn(const ros::SingleSubscriberPublisher &pub);
    void get_batt_discon(const ros::SingleSubscriberPublisher &pub);

    void get_io_tm_cb(const ros::TimerEvent &tmEvt);
    void get_io_conn(const ros::SingleSubscriberPublisher &pub);
    void get_io_discon(const ros::SingleSubscriberPublisher &pub);

    void get_alm_tm_cb(const ros::TimerEvent &tmEvt);
    void get_alm_conn(const ros::SingleSubscriberPublisher &pub);
    void get_alm_discon(const ros::SingleSubscriberPublisher &pub);

//private:
    ros::NodeHandle nh;

    ros::Subscriber sub_L_spd;
    ros::Subscriber sub_R_spd;
    ros::Subscriber sub_clr_alm;
    ros::Subscriber sub_set_io;
    ros::Subscriber sub_set_en;

    ros::Publisher pub_L_enc;
    ros::Publisher pub_R_enc;
    ros::Publisher pub_get_batt; 
    ros::Publisher pub_get_io; 
    ros::Publisher pub_get_alm; 

    ros::Timer tm_L_enc;
    ros::Timer tm_R_enc;
    ros::Timer tm_get_batt;
    ros::Timer tm_get_io;
    ros::Timer tm_get_alm;

}; //End of class SubscribeAndPublish

#undef TP_SET_L_SPD
#undef TP_SET_R_SPD 

#undef TP_SET_EN 
#undef TP_SET_IO 
#undef TP_CLR_ALM

#undef TP_GET_ENC_L 
#undef TP_GET_ENC_R 

#undef TP_GET_IO
#undef TP_GET_BAT 

#undef TIME_DUR_1HZ
#undef TIME_DUR_5HZ    
#undef TIME_DUR_10HZ   
#undef TIME_DUR_30HZ   

#endif // !DBH_THREAD_H__

dbh_thread.cpp

#include <ros/ros.h>
#include <sstream>
#include <boost/thread.hpp>
#include <std_msgs/UInt16.h>
#include "motor.h"
#include "dbh_thread.h"
// pub_test(pub, code, 1);
void pub_test(ros::Publisher & pub, std_msgs::UInt16  &code, uint16_t data){
    code.data = data;
    pub.publish(code);
}

void dbh_thread::set_L_speed_cb(const std_msgs::UInt16 &msg){
    ROS_INFO("set_L_speed_cb [%x]", msg.data);
    //ros::Rate loop_rate(30); 

    motor_set_L_speed(msg.data);
    //loop_rate.sleep();
}

void dbh_thread::set_R_speed_cb(const std_msgs::UInt16 &msg){
    ROS_INFO("set_R_speed_cb [%x]", msg.data);
    //ros::Rate loop_rate(30); 

    motor_set_R_speed(msg.data);
    //loop_rate.sleep();
}

void dbh_thread::set_en_brk_cb(const std_msgs::UInt16 &msg){
    ROS_INFO("set_en_brk_cb [%x]", msg.data);
    //ros::Rate loop_rate(30); 

    motor_set_enable(msg.data);
    // loop_rate.sleep();
}

void dbh_thread::set_io_cb(const std_msgs::UInt16 &msg){
    ROS_INFO("set_io_cb [%x]", msg.data);
    // ros::Rate loop_rate(30); 

    motor_set_io(msg.data);
    // loop_rate.sleep();
}

void dbh_thread::clr_alm_cb(const std_msgs::UInt16 &msg){
    ROS_INFO("clr_alm_cb [%x]", msg.data);
    motor_clr_alarm(msg.data);
}

void dbh_thread::L_enc_tm_cb(const ros::TimerEvent &tmEvt){
    std_msgs::UInt16  code;
    ROS_INFO("L_enc_tm_cb");
    if(motor_get_L_encode(&code.data)){
        pub_L_enc.publish(code);
    }
    pub_test(pub_L_enc, code, 1);
}

void dbh_thread::L_enc_conn(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("L_enc_conn");
    tm_L_enc.start();
} 

void dbh_thread::L_enc_discon(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("L_enc_discon");
    tm_L_enc.stop();
}

void dbh_thread::R_enc_tm_cb(const ros::TimerEvent &tmEvt){
    std_msgs::UInt16  code;
    ROS_INFO("R_enc_tm_cb");
    if(motor_get_R_encode(&code.data)){
        pub_R_enc.publish(code);
    }
    pub_test(pub_R_enc, code, 2);
}

void dbh_thread::R_enc_conn(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("R_enc_conn");
    tm_R_enc.start();
} 

void dbh_thread::R_enc_discon(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("R_enc_discon");
    tm_R_enc.stop();
}

void dbh_thread::get_batt_tm_cb(const ros::TimerEvent &tmEvt){
    std_msgs::UInt16  code;
    ROS_INFO("get_batt_tm_cb");
    if(motor_get_batt(&code.data)){
        pub_get_batt.publish(code);
    }
    pub_test(pub_get_batt, code, 3);
}

void dbh_thread::get_batt_conn(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("get_batt_conn");
    tm_get_batt.start();
} 

void dbh_thread::get_batt_discon(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("get_batt_discon");
    tm_get_batt.stop();
}

void dbh_thread::get_io_tm_cb(const ros::TimerEvent &tmEvt){
    std_msgs::UInt16  code;
    ROS_INFO("get_io_tm_cb");
    if(motor_get_io(&code.data)){
        pub_get_io.publish(code);
    }
    pub_test(pub_get_io, code, 4);
}

void dbh_thread::get_io_conn(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("get_io_conn");
    tm_get_io.start();
} 

void dbh_thread::get_io_discon(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("get_io_discon");
    tm_get_io.stop();
}

void dbh_thread::get_alm_tm_cb(const ros::TimerEvent &tmEvt){
    std_msgs::UInt16  code;
    ROS_INFO("get_alm_tm_cb");
    if(motor_get_alarm(&code.data)){
        pub_get_alm.publish(code);
    }
    pub_test(pub_get_alm, code, 5);
}

void dbh_thread::get_alm_conn(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("get_alm_conn");
    tm_get_alm.start();
} 

void dbh_thread::get_alm_discon(const ros::SingleSubscriberPublisher &pub){
    ROS_INFO("get_alm_discon");
    tm_get_alm.stop();
}

main.cpp

#include <ros/ros.h>
#include <pthread.h>
#include "dbh_thread.h"

int main(int argc, char **argv)
{
    //初始化節點
    ros::init(argc, argv, "dbh_serial_node");
    ros::Time::init();    // 這句看見ros demo 代碼裏面 在使用 ros::Duration(0.1)之前進行了初始化
                          // 加上在說
    //聲明節點句柄
    //ros::NodeHandle nh;

    dbh_thread thread;

    ros::MultiThreadedSpinner s; // 使用多線程
    ros::spin(s);
}