ROS智能轮椅项目-C++实现Socket通信功能包的编写

起因是老板接了个医专的可导航智能轮椅，要求写一个软件实现前后左右控制、扫描的地图保存显示，我在中间写了一个socket通信功能包实现接收前后左右指令发布线速度角速度、接收地图展示指令反馈gym图片。

先实现前后左右的功能，首先得了解socket，开启一个服务器，循环等待软件客户端的连接，连接成功后进入客户端读写的函数中，创建ROS节点句柄、消息发布者、话题名和消息类型。之后监听客户端发送的数据并进行数据的判断，初步定为1、2、3、4为前后左右。做完前后左右判断后对相应的线速度角速度进行赋值并发布到话题中。

具体代码：

#include <sstream>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
//socket need xx.h
#include<stdio.h>
#include<stdlib.h>
#include<errno.h>
#include<string.h>
#include<iostream>
#include <fstream>
#include<sys/types.h>
#include<netinet/in.h>
#include<sys/socket.h>
#include<sys/wait.h>
#include <unistd.h>
#define BUFFER_SIZE 100

using namespace std;
//文件发送
void SendFile(int m_Client){
    printf("开始发送");
	int haveSend = 0;
	const int bufferSize = 1024;
	char buffer[bufferSize] = {0};
	int readLen = 0;
	string srcFileName = "/home/bn/sanlou.pgm";
	ifstream srcFile;
	srcFile.open(srcFileName.c_str(),ios::binary);
	if(!srcFile){
		return;
	}
	while(!srcFile.eof()){
		srcFile.read(buffer,bufferSize);
		readLen = srcFile.gcount();
        //将char类型数据转化type类型发送给client
        unsigned char *buffer2 = (unsigned char*)buffer;
		send(m_Client,buffer,readLen,0);
		haveSend += readLen;	
	}
	srcFile.close();
	cout<<"send: "<<haveSend<<"B"<<endl;
}

//服务器读写方法
void listen_data(int fd,int id)
{

    //ros::init(argc, argv, "talker");
    // 创建节点句柄
    ros::NodeHandle n;  
    // 创建一个Publisher，发布名为chatter的topic，消息类型为geometry_msgs::Twist
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
    // 设置循环的频率
    ros::Rate loop_rate(10);
    int count = 0;


    char buffer[BUFFER_SIZE];
    while(1) //一直处于监听客户端信息状态,知道客户端断开或客户端发出退出信息
    {
        printf("client");
        memset(buffer,0,BUFFER_SIZE);
        int len = recv(fd,buffer,BUFFER_SIZE,0);
        if(len > 0) //如果接受到数据，则判断是否为退出信息
        {
            if(strcmp(buffer,"exit\n") == 0)
            {                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
                printf("id %d exited.\n",id);
                break;
            }
            printf("ID_%d:%s\n",id,buffer); //输出第N 个用户，输出的信息
            
        
            if (ros::ok())
            {
            // 初始化geometry_msgs类型的消息
            geometry_msgs::Twist vel;
            std::stringstream ss;
            ss << "hello world " << count;
            //msg.data = ss.str();

            //接受1，2,3,4,5.1-4对应控制车体前后左右移动。5对应加载地图上传到软件端
            const char* a = "1";
            const char* b = "2";
            const char* c = "3";
            const char* d = "4";
            const char* e = "5";
            if (strcmp(buffer,a) == 0)
            {
                vel.angular.z = 0;
                vel.linear.x = 1;
                vel.linear.y = 0;

            }
            else if (strcmp(buffer,b) == 0)
            {
                vel.angular.z = 0;
                vel.linear.x = 0;
                vel.linear.y = 1;
            }
            else if (strcmp(buffer,c) == 0)
            {
                vel.angular.z = 1;
                vel.linear.x = 0;
                vel.linear.y = 0;
            }
            else if (strcmp(buffer,d) == 0)
            {
                vel.angular.z = -1;
                vel.linear.x = 0;
                vel.linear.y = 0;
            }
            else if(strcmp(buffer,e) == 0)
            {
               SendFile(fd);
            }
            else
            {
                vel.angular.z = 0;
                vel.linear.x = 0;
                vel.linear.y = 0;
            }


            // 发布消息
            ROS_INFO("%f %f %f", vel.angular.z,vel.linear.x,vel.linear.y);
            vel_pub.publish(vel);

            // 循环等待回调函数
            ros::spinOnce();

            // 按照循环频率延时
            loop_rate.sleep();
            ++count;
            }  
            
        }
        else //接收数据小于0，或等于0 则退出
        {
            printf("clinet %d close!\n",id);
            break;
        }
    //如果服务端需要发送信息，此处添加发送信息
    //memset(buffer,0,BUFFER_SIZE);
    //scanf("%s",buffer);
    //send(fd,buffer,BUFFER_SIZE,0);
    }
    close(fd); //关闭此客户端的连接的socket
}

 

int main(int argc, char **argv)
{
    //printf("weq");

    ros::init(argc, argv, "talker");
    //sleep(1);
    int sockfd,new_fd;
    struct sockaddr_in my_addr; //本地连接socked
    struct sockaddr_in their_addr; //客户端连接socked
    unsigned int numbytes,sin_size;
    char buffer[BUFFER_SIZE];
    static int i=0; //记录连接客户端数目，可以使用数组，结构体等数据类型记录客户端信息（IP，端口等）
    //记录本地信息
    my_addr.sin_family = AF_INET; //IPV4
    my_addr.sin_port = htons(9999); //绑定端口9999，并将其转化为网络字节
    my_addr.sin_addr.s_addr = INADDR_ANY; //指定接收IP为任意（如果指定IP，则只接收指定IP）
    bzero(&(my_addr.sin_zero),0); //最后位置赋0，补位置
    //设置socked
    if((sockfd = socket(AF_INET,SOCK_STREAM,0)) == -1)
    {
        perror("socket error!\n");
        exit(1);
    }
    //绑定socked
    if((bind(sockfd,(struct sockaddr*)&my_addr,sizeof(my_addr))) < 0)
    {
        perror("bind error!\n");
        exit(1);
    }

    //开启监听,可连接客户端最大为10个
    if(listen(sockfd,10) == -1)
    {
        perror("listen error!\n");
        exit(1);
    }

    printf("打开监听成功！\n");
    //服务端一直运行，等待客户端连接
    while(1)
    {
        sin_size = sizeof(struct sockaddr_in);
        //等待客户端连接，连接后their_addr接收客户端的IP等信息，没有客户端连接，则一直等待
        if((new_fd = accept(sockfd,(struct sockaddr*)(&their_addr),&sin_size)) < 0)
        {
            perror("accept error!\n");
            exit(1);
        }
        //连接成功后，连接客户端数+1
        i++;
        //开启进程运行客户端
        pid_t childid;
        childid = fork(); //fork()函数有两个返回值，0为子进程，-1为错。子进程运行客户端
        if(childid == 0)
        {
            close(sockfd); //子进程中不再需要sockfd去监听，这里释放它，只需要new_fd即可
            listen_data(new_fd,i);
            exit(0);
        }
        //父进程继续执行while，在accept（）等待客户端。父进程的socked此时还在运行，没有关闭
        //此处没有设置父进程退出的代码，因为假设服务器一直运行，如果需要服务器自动退出，可设置服务器
        //等待连接的时间，如果一定时间没有客户端连接，可以退出等待，结束
    }
    //所有客户端
    close(sockfd);
    printf("server-------closed.\n");

  return 0;
}