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_OBJECT
public:
    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 information
struct sockaddr_in Client_addr;     //address information of connected machine
int 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()就是這個線程完成對中間這段程序的讀寫之後，進行解鎖。

所以我們在上面兩個地方用這三個函數，確保同時只有一個線程對其進行訪問和讀寫。

ROS中QThread的使用（同時進行topic的訂閱）

ROS中QThread的使用（同時進行topic的訂閱）

使用qtcreator時出現The specified source space "/home/xxx/src" does not exist的錯誤

使用qt時，出現error: undefined reference to。。。的原因（持續更新）

造成segment fault，產生core dump的可能原因

用CMake 構建Qt 項目

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結