ROS中QThread的使用（同时进行topic的订阅）

            最近在进行利用socket将一个topic上的位姿消息发送给UR5机器人的实验。由于socket时刻都处于接听的状态，类似一个死循环，另外由于只要接听的topic上一有消息来，就会调用callback函数，所以消息不断来时，这里也相当于一个死循环。因此就老运行不了。因此想到了使用一个多线程来进行这两部分的工作。

           由于在qtcreator里面进行编译，所以使用QThread类来进行。下面就来说明一下。

           我的node接听了一个其他node来的topic，在callback函数中，我将接听来的值给六个变量赋值。socket再读取这六个变量，将它进行一定处理后发送给UR5的控制器，使UR5进行运动。

           下面时socket.h头文件：

#ifndef SOCKET_H#define SOCKET_H#include "ros/ros.h"#include <qt4/QtGui/QApplication>#include <qt4/QtCore/QCoreApplication>#include <qt4/QtCore/QObject>#include <qt4/QtCore/QThread>//about socket#include <stdio.h>#include <iostream>#include <cstdlib>#include <unistd.h>#include <cerrno>#include <cstring>#include <sys/time.h>#include <sys/types.h>#include <sys/socket.h>#include <netinet/in.h>#include <arpa/inet.h>#include <sys/wait.h>#include <csignal>#include <qt4/QtCore/QMutex>class   QtROS:public QThread{    //Q_OBJECTpublic:    QtROS(int argc, char *argv[], const char* node_name);    virtual ~QtROS();    //ros::NodeHandle getNodeHandle(){return *n;}    int ursocket();    void run();private:    //ros::NodeHandle* n;};#endif // SOCKET_H

           在头文件中，我们声明一个类QtROS，让他继承QThread。里面包括：构造函数，析构函数，实例化一个句柄，一个socket的函数，一个run()。这个run函数极其重要，多线程中，QThread有一个槽函数start()，当触发它时，就会自动跳到run函数中进行执行。这里的Q_OBJECT要注释掉，否则用catkin_make编译时，会提示

/home/congleetea/catkin_ws/src/socket_to_ur5/src/socket.cpp:-1: error: undefined reference to `vtable for QtROS'

:-1: error: collect2: ld returned 1 exit status

的错误。具体原因设计到qt的编译原理，这里就不详述了。当然有它也可在CMakeLists.txt中进行一些QT的宏设置来进行编译。

      

          下面是socket.cpp文件：

#include "ros/ros.h"#include "std_msgs/String.h"#include "moveit_msgs/RobotTrajectory.h"#include "socket.h"#include <qt4/QtCore/QThread>#define PORT  30002#define HOSTIP "192.168.0.100"#define MAXDATESIZE 100#define BACKLOG 5               //客户端的最大数量using namespace std;struct sockaddr_in  Client;//int                 ursocket();int     ListenSocket;           //创建的socket返回的int      ConnectSocket;int      bindit;int      listento;struct sockaddr_in server;     //my address informationstruct sockaddr_in Client_addr;     //address information of connected machineint sin_size = sizeof(sockaddr_in);extern   float Jvalue0;extern   float Jvalue1;extern   float Jvalue2;extern   float Jvalue3;extern   float Jvalue4;extern   float Jvalue5;extern QMutex optimizer_mutex_;QtROS::QtROS(int argc, char *argv[], const char* node_name){    std::cout << "Initializing Node...\n";    ros::init(argc, argv, node_name);//    n = new ros::NodeHandle(node_name); //Use node name as Ros Namespace    ROS_INFO("Connected to roscore");}QtROS::~QtROS(){}int QtROS::ursocket(){    cout<<"welcome to socket!"<<endl;    memset(&server,0,sin_size);//初始化//create socket    ListenSocket = socket(AF_INET, SOCK_STREAM, 0);    if(ListenSocket == -1)                //if call socket fail    {        cout<<"Error  socket"<<strerror(errno)<<endl;        exit(1);    }    cout<<"socket is ok"<<endl;//bind socket    server.sin_family = AF_INET;           // host byte order , AF_INET = IPv4 Internet Protocols for Linux    server.sin_port = htons(PORT);         // sin_port is in short-network byte order,htons()=converts PORTNUMBER to network byte order    server.sin_addr.s_addr = inet_addr(HOSTIP);   // use my address automatically, use "INADDR_ANY"(0 so no need htons) or "inet_addr("192.120.13.1")    memset(&(server.sin_zero), '\0', sin_size);    //assign the address specified to the socket    bindit = bind(ListenSocket, (struct sockaddr *)&server, sin_size);    if (bindit == -1)    {//error check for bind()        cout <<"error bindit, because"<< strerror(errno)<<endl;        exit(1);    }    else        cout<<"bind is ok"<<endl;//listen socket    listento = listen(ListenSocket, BACKLOG);     //等待指定的端口出现客户端连接    if (listento == -1)    {//error check for listen()        cout <<"error listento, bacause "<< strerror(errno)<<endl;        exit(1);    }    cout<<"listen is ok!"<<endl;    return 0;}void QtROS::run(){    int i=1;    ursocket();    while(ros::ok())    {//accept a connection on listened socket        ConnectSocket = accept(ListenSocket, (struct sockaddr *)&Client_addr, (socklen_t*)&sin_size);//用于接受客户端的请求        if (ConnectSocket == -1)        {//error check for accept()            cout << "accept() has failed! because " << strerror(errno) << endl;            close(ConnectSocket);        }        cout << "Server_addr: got connection from " << inet_ntoa(Client_addr.sin_addr) << endl;        char pose[100]={0};        char movesign[100]={0};         //最好初始化=｛｝，否则会出现末尾乱码的现象。        char back[100]={0};        cout<<"time "<<i<<endl;        optimizer_mutex_.lock();//试图锁住互斥量，如果另一个线程已经锁住了这个互斥量，那么这次调用将阻塞知道那个线程把它解锁        cout<<"it is locked by socket"<<endl;        sprintf(pose,"(%.5f,%.5f,%.5f,%.5f,%.5f,%.5f)", Jvalue0,Jvalue1,Jvalue2,Jvalue3,Jvalue4,Jvalue5);        optimizer_mutex_.unlock();        cout<<"pose is :"<<pose<<endl;        recv(ConnectSocket,movesign,100,0);        cout<<"movesign is:"<<movesign<<endl;        if(movesign[0]=='a')        {            if(send(ConnectSocket,pose,100,0) == -1)            {                printf("write fail!\r\n");            }            printf("send ok!\r\n");            recv(ConnectSocket,back,100,0);            cout<<"back is :"<<back<<endl;        }        else            break;        close(ConnectSocket);        i++;    }    close(ListenSocket);    ros::spin();}

这里就是对头文件中的函数进行定义，int QtROS::ursocket()中进行socket的创建、绑定、接听。QtROS::QtROS(int argc, char *argv[], const char* node_name)中进行一些ROS的初始化，这里主要是说明生成的node。重要的是run函数中，一直在accept，接受请求、发送数据。

           下面是main.cpp:

#include "ros/ros.h"#include "std_msgs/String.h"#include "moveit_msgs/RobotTrajectory.h"#include <qt4/QtCore/QObject>#include <qt4/QtCore/QThread>#include "socket.h"             //用双引号，否则用<>可能找不到using namespace std;//class QtROS;float  Jvalue0 ;float  Jvalue1 ;float  Jvalue2 ;float  Jvalue3 ;float  Jvalue4 ;float  Jvalue5 ;QMutex optimizer_mutex_;void chatterCallback(const moveit_msgs::RobotTrajectory msg )//std_msgs::String::ConstPtr& msg{    if(optimizer_mutex_.tryLock())//tryLock试图锁住互斥量，没有锁住返回false。这里就是：如果被锁住。。。    {        for(int i = 0; i < msg.joint_trajectory.points.size(); i++)        {           trajectory_msgs::JointTrajectoryPoint tra = msg.joint_trajectory.points[i];           Jvalue0 = tra.positions[0];           Jvalue1 = tra.positions[1];           Jvalue2 = tra.positions[2];           Jvalue3 = tra.positions[3];           Jvalue4 = tra.positions[4];           Jvalue5 = tra.positions[5];           cout<<"Jvalue0~5: "<<Jvalue0<<"  "<<Jvalue1<<"  "<<Jvalue2<<"  "<<Jvalue3<<"  "<<Jvalue4<<"  "<<Jvalue5<<endl;        }        cout<<"it is finished"<<endl;            optimizer_mutex_.unlock();//试图锁住互斥量，如果另一个线程已经锁住了这个互斥量，那么这次调用将阻塞知道那个线程把它解锁            cout<<"it is unlocked"<<endl;            return;    }    cout<<"it is locked by main"<<endl;    cout<<"it is unlocked"<<endl;    return ;}int main(int argc, char **argv){  QtROS qtros(argc, argv, "socket_to_ur5_node");  QCoreApplication a(argc, argv);  ros::NodeHandle n;  ros::Subscriber sub = n.subscribe("/robot_trajectory", 100, chatterCallback);  qtros.start();  ros::spin();  a.exec();}

           重要的是main函数中的处理。首先，实例化一个  QtROS对象qtros，并说明生成的node名字为socket_to_ur5_node。然后实例化一个QCoreApplication对象。实例化订阅器订阅topic，并在callback函数中将接受的位姿进行赋值。

          这里会出现的问题是segment fault ,core dump的错误。原因就是callback函数对Jvalue0～Jvalue5进行访问写值时，run函数又在读取这些值进行发送，所以就出现了这个错误。因此，我们在两个地方的Jvalue0～Jvalue5处要进行互锁操作，这就涉及到QT中类QThread的使用。这个类的成员函数如下：

最重要的是lock ()，tryLock ()和unlock ()的使用。

              tryLock ()试图锁住互斥量（自己可以访问，其他线程不能访问），如果这里已经被其他线程锁住了，那么他就没有锁住，返回false。

              lock()锁住互斥量，如果被其他线程锁住（正被另一个线程访问），那就等待，直到被解锁。

           unlock()就是这个线程完成对中间这段程序的读写之后，进行解锁。

               所以我们在上面两个地方用这三个函数，确保同时只有一个线程对其进行访问和读写。