QT与ROS通过TCP通信指挥小车运动

---

项目做完有段时间了，很多细节都不太记得了，所以写得很草，细节可以看代码。 长个记性，下次一定要及时记录！

---

效果展示

QT与ROS通过TCP通信指挥小车运动

本文主要实现：
1、ROS作为主机（客户端），qt作为从机（服务器）进行TCP通信，通过从机给小车发布任务，在主机上实时显示小车位置、小车状态信息
2、用小乌龟代替小车，进行矩形轨迹运动

---

准备工作

1、开发环境说明

		 - ubuntu 18.04以及ROS Melodic环境
		 -  ubuntu下的qt creator（随机版本应该都行）
		 注：双系统或者虚拟机都行

2、代码说明

			1）IES为qt部分代码，直接打开.pro文件即可
			2）src部分为ROS的代码，需要创建自己的工作空间再进行编译
			3）运行：一定得先打开服务器（qt程序），再运行
			roslaunch my_tcp ros_tcp.launch
			注：这部分涉及到一些ros运行的基础知识

---

一、TCP协议

• 本文由于是ROS与qt通信，在ROS部分，我使用的是C++编程，qt使用其自带的库函数，所以需要先了解一些C++部分编写协议的知识
  https://blog.csdn.net/cpp_learner/article/details/127813889
• 先确定客户端和服务器，一方需要直接收接受小车位置状态信息，并且偶尔发布任务信息；一方需要一直发布小车位置状态信息，不定时接收任务，但需要一直等待（后面编程的时候发现C++写tcp协议有阻塞，等待时不能发送）。所以我选择ROS作为主机（客户端），qt作为从机（服务器）

二、搭建ros（客户端）方面的通讯

首先得建立自己的ros工作空间，然后在工作空间里面去添加节点使用TCP套接字（sockets）来发送和接收数据

1、建立客户端的流程

1、初始化TCP套接字，socket（）获得一个sockfd。注意第二个参数必须SOCK_STREAM.
2、设置服务器地址结构，按通信地址请求连接（必须服务器的）
3、连接成功后，进行写或读的操作。（开放了端口，允许客户端连接）
4、关闭sockfd。

2、tcp_client.cpp

功能：订阅小车的位置和速度信息，在回调里面获取，并通过tcp协议发送给qt
注意：两种信息的格式
小车速度话题 ：/turtle1/cmd_vel geometry_msgs::Twist
小车位置话题 ：/turtle1/pose turtlesim::Pose
主要代码：

void carposeCallback(turtlesim::Pose poseMsg)//位置回调函数
 {
        carpose_x=poseMsg.x;
        carpose_y=poseMsg.y;
        //std::cout << "carposeCallback" << std::endl;
 }
 void carspeedCallback(geometry_msgs::Twist speedMsg)//速度回调函数
 {
        carspeed=speedMsg.linear.x;
 }

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tcp_client");
    ros::NodeHandle n;
    ros::param::get("~client_port",port);//将参数引入launch文件，方便随时修改
    ros::param::get("~client_ip",ip);

    ros::Subscriber sub_carpose = n.subscribe("/turtle1/pose", 1000, carposeCallback);//订阅小乌龟位置信息
    ros::Subscriber sub_carspeed = n.subscribe("/turtle1/cmd_vel", 1000, carspeedCallback);//订阅小乌龟速度信息
    
//TCP通信部分
    //切片发送字符串的lambda函数
    auto sendString = [](int& socket, int step,std::string& send_str)
    {
      //计算待发送数据的长度
      int len = strlen(send_str.c_str());
      //发送数据
      for (int i = 0; i < len; i = i + step) 
      {
        std::string str = send_str.substr(i, step);
        send(socket, str.c_str(), strlen(str.c_str()), 0);
        usleep(1);//中间需要有停顿，不可以一直发送，否则server接受会出现错乱
      }
      //发送一个空字符，server收到长度为0的字符后会停止接收
      std::string str = "";
      send(socket, str.c_str(), strlen(str.c_str()), 0);  
    };
      //创建客户端套接字
      int sock = socket(AF_INET, SOCK_STREAM, 0);
      //向服务器（特定的IP和端口）发起请求
      struct sockaddr_in serv_addr;
      memset(&serv_addr, 0, sizeof(serv_addr));           //每个字节都用0填充
      serv_addr.sin_family = AF_INET;                     //使用IPv4地址
      serv_addr.sin_addr.s_addr = inet_addr(ip.c_str());  //具体的IP地址
      serv_addr.sin_port = htons(port);          //端口

       //连接服务器
      //connect只会链接一次，不会一直等待
       if (connect(sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
     {
        std::cout << "can not connect socket" << std::endl;
        exit(1);
      } 

    //成功连接，在while循环内持续创建套接字，并且发送信息
    while (true) 
    {
     // 查看一次回调函数队列
    //  ros::spinOnce();
      std::cout << "successful connect " << std::endl;


     // 从服务器接收数据
        char buffer[1024];
        // if(strcmp(buffer,"0")=0)
        // {
          memset(buffer, '\0', sizeof(buffer));
          ssize_t bytesReceived = read(sock, buffer, sizeof(buffer));
    
        if (bytesReceived == -1)
         {
            std::cout <<"recv:Failed to receive data from the server." << std::endl;
            close(sock);
            return 0;
         } 
         else
          {
            std::cout << "client：Received " << bytesReceived << " bytes of data from the server:" << std::endl;
            buffer[bytesReceived] = '\0'; // 确保字符串以空字符结尾
            std::cout << buffer << std::endl;
          }
          
          //忘了这个判断是怎么定义的了
           while((strcmp(buffer,"0")!=0)||(carpose_x!=8.7&&carpose_y!=6.7))
                {
                ros::spinOnce();
                //向server发送信息
                // write(socket_fd,flag,sizeof(flag));
                std::ostringstream stream;
                stream << carpose_x << " " << carpose_y << " " << carspeed <<" ";
                std::string result = stream.str();
                sendString(sock, 1024, result);
                std::cout << "send "<<result << std::endl;
                // sleep(1);//不要用sleep函数来休眠，sleep的参数整数有效，不够精准
                // 睡眠0.9秒
                std::this_thread::sleep_for(std::chrono::milliseconds(120));
                }
          
      // close(sock);
    }
    close(sock);  
}

3、tcp_client_recv.cpp

功能：接受任务指令，订阅小车的位置信息，创建发布者控制速度，通过tcp让小车执行指令

主要代码：

// 回调函数，获取当前小乌龟的位置信息
void poseCallback(turtlesim::Pose poseMsg)
{
    current_pose = poseMsg;
}

void drawsqure(double side_length,double side_width)//小乌龟画方形
{
    // 设置运动速度和角速度
    double linear_speed = -1.2;   // 线速度为1.0单位/s,逆时针
    double angular_speed = 0.6;  // 角速度为0.5单位/s
    // 设置正方形边长和旋转次数（四个角）
    // int side_length = 2.0;   // 正方形边长为2.0单位
    int num_rotations = 4;      // 旋转次数为4
    // 创建一个Twist消息对象，用于控制小乌龟的运动
    geometry_msgs::Twist vel_msg;
    int j=0;
    // 开始画正方形
    for (int i = 0; i < num_rotations; i++)
    {
        // 移动一段距离（正方形边长）
        double distance_moved = 0.0;
        ros::Rate loop_rate(10);  // 控制循环频率为10Hz
        double distance=0;
        if(i==0||i==2)
        {
            distance=side_length;
        }
        if(i==1||i==3)
        {
            distance=side_width;
        }
        turtlesim::Pose initial_pose=current_pose;
        vel_msg.linear.x = linear_speed;
        vel_msg.angular.z = 0.0;
        // velocity_publisher.publish(vel_msg);
        while ((distance_moved < distance))
        {
            // 设置线速度和角速度
            velocity_publisher.publish(vel_msg);
            // for(int i=0;i<5;i++)
            loop_rate.sleep();
            // 计算已移动的距离（欧氏距离）
            distance_moved = sqrt(pow(current_pose.x-initial_pose.x, 2) + pow(current_pose.y-initial_pose.y, 2));
            std::cout << "distance_moved "<<distance_moved << std::endl;
            // if(i==0&&distance_moved >= distance)
            //     j++;
        }
        // 停止小乌龟的运动
        vel_msg.linear.x = 0.0;
        velocity_publisher.publish(vel_msg);
        ros::Duration(1).sleep();  // 停顿1秒钟
        // 进行旋转90°
        double initial_angle = current_pose.theta;
        // std::cout << "distance "<<distance << std::endl;
        double angle_turned = 0.0;
        vel_msg.angular.z = angular_speed;
        // velocity_publisher.publish(vel_msg);
        while (angle_turned < PI /rotate||angle_turned>5)   // 弧度表示90°角
        {
            // vel_msg.linear.x = 0.0;
            // vel_msg.angular.z = angular_speed;
            velocity_publisher.publish(vel_msg);
            loop_rate.sleep();
            angle_turned = std::abs(current_pose.theta - initial_angle);
            std::cout << "initial_angle "<<initial_angle << std::endl;
            std::cout << "angle_turned "<<angle_turned << std::endl;
        }
        // 停止小乌龟的运动
        vel_msg.angular.z = 0.0;
        velocity_publisher.publish(vel_msg);
        ros::Duration(1).sleep();  // 停顿1秒钟
    }
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"car_control");    // 初始化ROS节点
    ros::NodeHandle nh;// 初始化ROS句柄
    ros::param::get("~side_length",side_length);
    ros::param::get("~side_width",side_width);
    ros::param::get("~rotate",rotate);
    ros::param::get("~client_port",port);
    ros::param::get("~client_ip",ip);
    ros::Publisher pose_pub = nh.advertise<turtlesim::Pose>("/turtle1/pose",10);    // 创建发布者
    velocity_publisher = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ros::Subscriber pose_subscriber = nh.subscribe("/turtle1/pose", 1000, poseCallback);// 创建一个Subscriber订阅小乌龟的位置信息
  
    //去使用小乌龟自带的服务，使其路径轨迹为白色
    ros::ServiceClient initialposeClient = nh.serviceClient<turtlesim::TeleportAbsolute>("/turtle1/teleport_absolute");
    ros::ServiceClient setpenClient = nh.serviceClient<turtlesim::SetPen>("/turtle1/set_pen");
    ros::service::waitForService("/turtle1/set_pen");
    turtlesim::SetPen::Request pen_req;
    turtlesim::SetPen::Response pen_resq;
    pen_req.r=255;
    pen_req.g=255;
    pen_req.b=255;
    pen_req.width=4;
    pen_req.off=1;
    bool ok=setpenClient.call(pen_req,pen_resq);
    if(ok)
     {
            ROS_INFO("whitepen");
    }
    else
        ROS_INFO("whitepen failed");

    //去使用小乌龟自带的服务，定义初始点位置
    ros::service::waitForService("/turtle1/teleport_absolute");
    turtlesim::TeleportAbsolute::Request req;
    turtlesim::TeleportAbsolute::Response resp;
    req.x=8.7;
    req.y=6.7;
    req.theta=0;
    ok = initialposeClient.call(req, resp);
    if (ok)
     {
            ROS_INFO("initialpose");
    }
    
    ros::AsyncSpinner spinner(2);
    spinner.start();

    turtlesim::Pose pose_msg;    // 声命变量类型，几何消息类型
    pose_msg.x = 0.0;    // 给变量赋值，线速度和角速度
    pose_msg.y = 0.0;
    ros::Rate r(10);// 设置发布频率，10Hz

//TCP协议部分
    int sockfdTCP = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfdTCP == -1)
    {
        printf("Failed to create socket.\n");
        return 0;
    }
    
    // 设置服务器地址信息
    struct sockaddr_in serverAddr{};
    serverAddr.sin_family = AF_INET;                     //使用IPv4地址
    serverAddr.sin_addr.s_addr = inet_addr(ip.c_str());  //具体的IP地址
    serverAddr.sin_port = htons(port);          //端口
    // serverAddr.sin_family = AF_INET;
    // serverAddr.sin_port = htons(port); //端口号,根据你实际要连接的tcp服务端的端口设置
    // inet_pton(AF_INET, "127.0.0.1", &serverAddr.sin_addr); //服务端ip
    
    // 连接到服务器
    if (connect(sockfdTCP, reinterpret_cast<struct sockaddr*>(&serverAddr), sizeof(serverAddr)) == -1) 
    {
        printf("recv:Failed to connect to the server.\n");
        close(sockfdTCP);
        return 0;
    }
    
    while (ros::ok())
    {
        // 发布消息
        pose_pub.publish(pose_msg);
        // 添加延时函数，达到设置发布频率目的
        r.sleep();
        // 官方建议的回头函数，回头一次
        // ros::spinOnce();


         // 从服务器接收数据
        char buffer[1024];
        memset(buffer, '\0', sizeof(buffer));
        ssize_t bytesReceived = read(sockfdTCP, buffer, sizeof(buffer));
    
        if (bytesReceived == -1)
         {
            std::cout <<"recv:Failed to receive data from the server." << std::endl;
            close(sockfdTCP);
            return 0;
         } 
         else
          {
            std::cout << "Received " << bytesReceived << " bytes of data from the server:" << std::endl;
            buffer[bytesReceived] = '\0'; // 确保字符串以空字符结尾
            std::cout << buffer << std::endl;
          }
          // 关闭socket连接
        // close(sockfdTCP);

//接收到信息后进行分析，有三个任务例子
        if(strcmp(buffer,"1")==0)
        {
            std::cout << "A" << std::endl;//a为三个正方形大小的路径
            drawsqure(side_length*3,side_length);
            ros::service::waitForService("/turtle1/teleport_absolute");//因小乌龟矩形走得不标准，所以每个任务结束后需要重新设置起始点
            req.x=8.7;
            req.y=6.7;
            req.theta=0;
            bool ok = initialposeClient.call(req, resp);
            if (ok)
            {
                    ROS_INFO("initialpose");
            }
        }
        else if(strcmp(buffer,"2")==0)
        {
            std::cout << "B" << std::endl;//b为两个正方形大小的路径
            drawsqure(side_length*2,side_length);
            ros::service::waitForService("/turtle1/teleport_absolute");
            req.x=8.7;
            req.y=6.7;
            req.theta=0;
            bool ok = initialposeClient.call(req, resp);
            if (ok)
            {
                    ROS_INFO("initialpose");
            }
        }
        else if(strcmp(buffer,"3")==0)
        {
            std::cout << "C" << std::endl;
            drawsqure(side_length,side_length);
            ros::service::waitForService("/turtle1/teleport_absolute");
            req.x=8.7;
            req.y=6.7;
            req.theta=0;
            bool ok = initialposeClient.call(req, resp);
            if (ok)
            {
                    ROS_INFO("initialpose");
            }
        }
    }
    // 关闭socket连接
    close(sockfdTCP);
    
    spinner.stop();
    return 0;
}	

三、qt方面的服务器搭建

1、使用mainwindow基类，调用network库

在.pro文件里面加入QT       += sql network

2、设计ui界面

3、创建服务器

(1)先用 Open()函数打开等待传输的可读文件;
(2)用 Socket()创建套接口,并给套接口地址结构赋值;
(3)用 Bind()函数绑定套接口;
(4)用 Listen()函数在该套接口上监听请求;
(5)用 Accept()函数接受请求,产生新的套接口及描述字,并与客户端连
接;
(6)最后用 Write()函数将读取的数据存放在内存中,向客户段发送数据;
(7)传输完毕时,用 Close()关闭所有进程,结束文件传输。
注意：checkbox的三态

四、ros小车部分

使用小海龟代替小车，为了达到设计要求，需要对turtlesim的源码进行更改。
 （没写怎么做就是忘了具体改的哪儿了）

1）替换小乌龟图片；
把images里面的图片全部替换成这个：

2）更改小乌龟初始位置；（通过服务实现）
3）更改背景板；
4）绘制简易地图；
在turtle_frame.cpp中把函数替换为如下

 void TurtleFrame::paintEvent(QPaintEvent*)
{
  QPainter painter(this);

  painter.drawImage(QPoint(0, 0), path_image_);

  painter.drawRect(100,200,100,100);
  painter.drawRect(200,200,100,100);
  painter.drawRect(300,200,100,100);
  // QPen pen(QColor(255, 255, 255));
  // painter.setPen(pen); 

  M_Turtle::iterator it = turtles_.begin();
  M_Turtle::iterator end = turtles_.end();
  for (; it != end; ++it)
  {
    it->second->paint(painter);
  }
}

5）编写launch文件启动小乌龟。

五、qt小车部分

1）地图绘制：使用Qpianter的 drawRect 函数来绘制三个矩形，在自定义的类中重写paintevent函数，他会在窗口需要重绘时被调用
2）小车随路径移动：利用QTimer来周期性地更新图片的位置，实现图片的移动效果
具体可以看代码如何实现

小结

存在的问题：
1、本质上我并没有把TCP的协议理解透彻，当qt处于windows系统下，ROS用虚拟机时（模拟不同电脑之间），我并不知道他两端口号和IP如何去配置
2、用C++去写TCP协议的时候，发现TCP通信有阻塞，尝试用异步处理回调，多线程，中断等方法都未解决
3、视频中ROS部分实现的小车（其实就是小乌龟）移动控制不够精准，更推荐的方式是自己搭建小车模型赛道，用三维进行仿真（不过我这个任务那样搞太复杂了，其实是搞不出来）

代码下载

链接：https://pan.quark.cn/s/afbe79d4040b
提取码：Lrxp