The reason is that the boss picked up a navigable smart wheelchair from a medical school, and asked to write a software to achieve front, rear, left, and right control, and save and display the scanned map. The map display command feeds back the gym picture.
To realize the functions of front, back, left, and right, you must first understand the socket, open a server, and wait for the connection of the software client in a loop. After the connection is successful, enter the function of reading and writing on the client, and create the ROS node handle, message publisher, topic name, and message type. After that, listen to the data sent by the client and judge the data, and initially set 1, 2, 3, and 4 as front, back, left, and right. After making the front, back, left, and right judgments, assign the corresponding linear velocity and angular velocity and publish it to the topic.
Specific code:
#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;
}