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);
}