一、测试说明
- 项目需要两台主机（视觉端Nvidia AGX Xavier；控制端Intel NUC10i7）进行机器人位姿、关节指令等double数据传输，计划使用socket通信实现；
- 两台主机通过一条网线建立局域网，分别创建新的有线连接：
服务器端：ipv4：192.168.56.3；子网掩码：255.255.255.0；网关：192.168.56.1
客户端：ipv4：192.168.56.2；子网掩码：255.255.255.0；网关：192.168.56.1 - 服务器端先启动，后启动客户端；
- 一定要长时间测试双方通信过程中的收发延时以及数据丢失情况；
二、数据传输实时性及稳定性提升方案
- 巨帧（jumbo frame）
# MTU(最大传输单元)的缺省值为1500，通过下面命令将其改为9000
# 每次开机都要重新设置，可设置开机自启动脚本
ifconfig eth0 mtu 9000
经测试，mtu默认保持1500时，发送端发送10000次数据，接收端只能收到300左右。将mtu改为9000后基本消除这个问题。
- 更改双方收发缓冲区大小
缓冲区大小计算公式：链接带宽（link_bandwidth）* 往返时间（RTT）/ 8；
例如，如果应用程序是通过一个 100Mbps 的局域网进行通信，其 RRT 为 50 ms，那么缓冲区就是：100 MBps * 0.050 sec / 8 = 0.625 MB = 625 KB
在代码中设置缓冲区大小时，需要转换量级：625 KB * 1024 = 640,000 Byte
实际测试发现，计算出的缓冲区大小往往只有几十KB（详见代码附录）
# 查询链接带宽，打印结果中有一项关于speed的信息即为带宽
sudo apt install ethtool
ethtool 网卡名称
# 往返时间RRT查询方法
ping ip地址
- 设置双方缓冲区大小
int ret, sock, sock_buf_size;
sock = socket( AF_INET, SOCK_STREAM, 0 );
sock_buf_size = BDP;
ret = setsockopt( sock, SOL_SOCKET, SO_SNDBUF,
(char *)&sock_buf_size, sizeof(sock_buf_size) );
ret = setsockopt( sock, SOL_SOCKET, SO_RCVBUF,
(char *)&sock_buf_size, sizeof(sock_buf_size) );
三、C++代码
- 服务器端：
/*
* @Author: XUYANG
* @Date: 2022-02-18 15:07:04
* @LastEditTime: 2022-02-18 17:59:15
* @LastEditors: XUYANG
* @Description:
* @FilePath: \src\auto_charging_system\robot_planner\src\socket_proc.cpp
*/
//#include <robot_planner/socket_proc.h>
#include <iostream>
#include <vector>
#include <string.h>
#include <thread>
#include <chrono>
#include <sstream>
#include <stdio.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include <fstream>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/time.h>
#include <math.h>
#include <iomanip>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
using std::cout;
using std::endl;
using std::shared_ptr;
using std::string;
using std::vector;
using namespace std;
using namespace std::chrono;
using std::perror;
#define _MYPORT_ROBOT_ 9054 // 9054
#define _BACKLOG_ROBOT_ 12
#define _BUFFER_SIZE_ 2048 // 70 1024
class NetRobot
{
public:
NetRobot();
~NetRobot();
void mainLoop();
int socketRecvRun();
// int socketSendRun();
int socketSendRun(vector<double> &commands, int flag);
bool isConnectionOK();
void parseCommand();
int initSocketServer();
bool waitForConnection();
vector<double> joint_positions = vector<double>(6, 0.0);
vector<double> joint_speeds = vector<double>(6, 0.0);
vector<double> joint_currents = vector<double>(6, 0.0);
vector<double> end_effector_pose = vector<double>(6, 0.0);
bool is_robotInit_ok_ = false;
struct timeval tv;
int recv_counts = 0;
int send_counts = 0;
ofstream outfile1;
bool data_flag = true;
/* 在sock_fd上进行监听，new_fd 接受新的连接 */
int sock_fd_, new_fd_;
/* 自己的地址信息 */
struct sockaddr_in my_addr_;
/* 连接者的地址信息*/
struct sockaddr_in their_addr_;
int sin_size_;
/* 从buffer中读取的位数 */
int buf_bytes_;
/* buffer */
char buf_[_BUFFER_SIZE_];
char buf_send_[_BUFFER_SIZE_];
bool is_connection_ok_;
};
NetRobot::NetRobot()
{
sin_size_ = sizeof(struct sockaddr_in);
/*
create ......
*/
}
NetRobot::~NetRobot()
{
outfile1.close();
/*
delete ......
*/
}
int NetRobot::initSocketServer()
{
outfile1 = ofstream("/home/vision/Documents/socket_data/server_data_01.txt", ios::trunc);
/* 如果调用 socket() 出错，则退出 */
if ((sock_fd_ = socket(AF_INET, SOCK_STREAM, 0)) == -1)
{
/* 输出错误提示并退出 */
perror(" socket_robot");
return -1;
}
#if 1
// 设置缓冲区大小
int socket_buf_size = 8192; // 16 KB：16384； 8 KB: 8192
setsockopt(sock_fd_, SOL_SOCKET, SO_SNDBUF, (char *)&socket_buf_size, sizeof(socket_buf_size));
setsockopt(sock_fd_, SOL_SOCKET, SO_RCVBUF, (char *)&socket_buf_size, sizeof(socket_buf_size));
#endif /*
#if 0
// 禁用nagle算法，最小化报文传输延时
int flag = 1;
if ( setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(flag)) == -1 )
{
cout << "couldn't setsockopt(TCP_NODELAY)" << endl;
return -1;
}
#endif /*
/* 主机字节顺序 */
my_addr_.sin_family = AF_INET;
/* 网络字节顺序，短整型 */
my_addr_.sin_port = htons(_MYPORT_ROBOT_); // 9054
/* 将运行程序机器的IP填充入s_addr */
my_addr_.sin_addr.s_addr = INADDR_ANY; // INADDR_ANY 0.0.0.0 monitor all
/* 将此结构的其余空间清零 */
bzero(&(my_addr_.sin_zero), 8);
/* 这里是我们一直强调的错误检查！！ */
int iSockOptVal = 1;
/* 检测是否端口备占用 */
if (setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &iSockOptVal, sizeof(iSockOptVal)) == -1)
{
perror("setsockopt fail");
close(sock_fd_);
return -1;
}
if (bind(sock_fd_, (struct sockaddr *)&my_addr_, sizeof(struct sockaddr)) == -1)
{
/* 如果调用bind()失败，则给出错误提示，退出 */
perror(" bind_robot ");
return -1;
}
/* 这里是我们一直强调的错误检查！！ */
if (listen(sock_fd_, _BACKLOG_ROBOT_) == -1)
{
/* 如果调用 listen 失败，则给出错误提示，退出 */
perror("listen_robot");
return -1;
}
// ROS_INFO("Socket server initialized");
cout << "Socket server initialized" << endl;
return 0;
}
/* 等待Socket客户端连接，连接成功返回true；失败返回false*/
bool NetRobot::waitForConnection()
{
/* 接收新的连接，若无连接请求，则线程睡眠等待连接请求 */
// ROS_INFO("Waiting for new connection");
cout << "Waiting for new connection" << endl;
new_fd_ = accept(sock_fd_, (struct sockaddr *)&their_addr_, (socklen_t *)&sin_size_);
/* 若accept返回为-1，则连接错误，重新等待连接 */
if (new_fd_ < 0)
{
perror(" accept_robot ");
return false;
}
else
{
char *ip = inet_ntoa(their_addr_.sin_addr);
// ROS_INFO("Client@%s connected\n", ip);
cout << "Client@" << ip << " connected!" << endl;
#if 1
/* 用非阻塞方式 */
fcntl(new_fd_, F_SETFL, O_NONBLOCK);
/* for send/receive map */
#endif
return true;
}
}
bool NetRobot::isConnectionOK()
{
if (initSocketServer()) // 初始化socket，监听连接请求
return false; // 初始化有问题
is_connection_ok_ = waitForConnection(); // 等待连接请求
close(sock_fd_); // 建立连接后，关闭监听socket
return is_connection_ok_; // 初始化过程已完成
}
// 对buf_接收到的指令进行解析
void NetRobot::parseCommand()
{
if(data_flag == false)
{
data_flag = true;
return;
}
// *(double* )(buf_+8*i)
cout << "!!! recv_counts: " << recv_counts << endl;
cout << *(double* )buf_ << endl;
// if (buf_[0] == 'A') // 解析1：依次返回每个关节的位置（°）、速度（m/s）、电流值（A）
if (fabs(*(double* )buf_ - 1111.1111) < 1e-4)
{
// 前8字节：标志位； 9-56字节：1-6关节角度值；
double *pPosition = (double *)(buf_ + 8);
for (int i = 0; i < 6; i++) // 将位置信息存入指定容器中
{
joint_positions[i] = pPosition[i];
}
cout << "yyds..." << endl;
outfile1 << recv_counts << " " << joint_positions[0] << " " << joint_positions[1] << " "
<< joint_positions[2] << " " << joint_positions[3] << " "
<< joint_positions[4] << " " << joint_positions[5] << endl;
}
else if (fabs(*(double* )buf_ - 2222.2222) < 1e-4)
{
// 前8字节：标志位； 9-56字节：1-6关节速度值；
double *pSpeed = (double *)(buf_ + 8);
for (int i = 0; i < 6; i++) // 将速度信息存入指定容器中
{
joint_speeds[i] = pSpeed[i];
}
cout << "yyds..." << endl;
outfile1 << recv_counts << " " << joint_speeds[0] << " " << joint_speeds[1] << " "
<< joint_speeds[2] << " " << joint_speeds[3] << " "
<< joint_speeds[4] << " " << joint_speeds[5] << endl;
}
else if (fabs(*(double* )buf_ - 3333.3333) < 1e-4)
{
// double *pCurrent = (double *)(buf_);
// joint_currents[0] = pCurrent[0];
// cout << "yyds..." << endl;
// outfile1 << recv_counts << " " << joint_currents[0] << endl;
#if 1
// // 前8字节：标志位； 9-56字节：1-6关节电流值
double *pCurrent = (double *)(buf_ + 8);
for (int i = 0; i < 6; i++) // 将电流信息存入指定容器中
{
joint_currents[i] = pCurrent[i];
}
// cout << "yyds..." << endl;
outfile1 << recv_counts << " " << joint_currents[0] << " " << joint_currents[1] << " "
<< joint_currents[2] << " " << joint_currents[3] << " "
<< joint_currents[4] << " " << joint_currents[5] << endl;
// for (int i = 0; i < 6; i++)
// {
// cout << setiosflags(ios::fixed) << setprecision(6) << joint_currents[i] << " ";
// }
// cout << endl;
#endif
}
// else if (buf_[0] == 'B') // 解析2：初始状态下机械臂已就位，告诉视觉端,机械臂已就位
else if (fabs(*(double* )buf_ - 4444.4444) < 1e-4)
{
is_robotInit_ok_ = true;
}
// else if (buf_[0] == 'C') // 解析3：返回机械臂末端位姿：平移分量，旋转分量
else if (fabs(*(double* )buf_ - 5555.5555) < 1e-4)
{
//
double *pEEpose = (double *)(buf_ + 8);
for (int i = 0; i < 6; i++) // 将末端位姿信息存入指定容器中
{
end_effector_pose[i] = pEEpose[i];
}
// for (int i = 0; i < 6; i++)
// {
// cout << end_effector_pose[i] << " ";
// }
// cout << endl;
}
recv_counts++;
}
int NetRobot::socketRecvRun()
{
memset(buf_, 0, _BUFFER_SIZE_);
buf_bytes_ = recv(new_fd_, buf_, _BUFFER_SIZE_, 0);
// cout << "!!! " << buf_bytes_ << endl;
/* 客户端连接关闭，结束连接，等待新的连接请求 */
if (buf_bytes_ == 0)
{
// gettimeofday(&tv, NULL);
// printf("millisecond: %ld\n", tv.tv_sec * 1000 + tv.tv_usec / 1000); // 毫秒
// printf("microsecond: %ld\n", tv.tv_sec * 1000000 + tv.tv_usec); // 徽秒
// ROS_WARN("Socket client has disconnected, close current connection");
cout << "Socket client has disconnected, close current connection." << endl;
data_flag = false;
close(new_fd_);
is_connection_ok_ = false;
return 0;
}
/* 接收异常，结束连接，等待新的连接请求 */
if (buf_bytes_ < 0)
{
if (errno == EWOULDBLOCK)
{
data_flag = false;
// 内核缓冲区没有数据了
cout << "there is no data available now." << endl;
outfile1 << "111111" << endl;
}
else if (errno == EINTR)
{
data_flag = false;
// 被信号中断了，需要重新尝试发送
cout << "recv data interrupted by signal." << endl;
outfile1 << "222222" << endl;
}
else
{
// 出现了严重错误
cout << "Socket receiving error, close current connection." << endl;
data_flag = false;
close(new_fd_);
is_connection_ok_ = false;
return 0;
}
}
/* 接收正常，将buffer最后一位的\n改成\0 */
if (buf_bytes_ > 0)
{
// 视情况判断这部分是否保留
#if 0
if (buf_[buf_bytes_ - 1] == '\n')
{
buf_[buf_bytes_ - 1] = '\0';
buf_bytes_--;
}
#endif
// cout << "Received data: ";
// for (int i = 0; i < 6; i++)
// {
// cout << *(double* )(buf_+8*i) << " ";
// }
// cout << endl;
}
return 1;
}
int NetRobot::socketSendRun(vector<double> &commands, int flag)
{
// 要发送什么东西，先存在buf_send_中！
memset(buf_send_, 0, _BUFFER_SIZE_);
// double *pData = (double *)(buf_send_ + 8);
double *pData = (double *)buf_send_;
if (flag == 0) // 指令1：末端期望位姿; 旋转分量用四元数表示还是什么呢？
{
pData[0] = 1111.1111;
for (int i = 0; i < 6; i++) // 平移+旋转
{
if (i == 0)
{
pData[i+1] = send_counts;
continue;
}
pData[i+1] = commands[i];
}
}
int ret = send(new_fd_, buf_send_, _BUFFER_SIZE_, 0);
if (ret == -1)
{
if (errno == EWOULDBLOCK)
{
// 非阻塞模式下send函数由于TCP窗口太小发不出去数据
cout << "send data error as TCP window size is too small." << endl;
// 需要判别是否需要发送！！！
return 0;
}
else if (errno == EINTR)
{
// 被信号中断
cout << "sending data interrupted by signal." << endl;
// 需要判别是否需要发送！！！
return 0;
}
else
{
// 出现严重错误！！
cout << "send data error." << endl;
close(new_fd_);
return -1;
}
}
else if (ret == 0)
{
// 客户端关闭了连接
cout << "the client down, send data error." << endl;
close(new_fd_);
return -1;
}
// cout << "send the msg to robot successfully!" << endl;
cout << "!!! send_counts: " << send_counts << endl;
send_counts++;
return 0;
}
int main(int argc, char **argv)
{
NetRobot* netRobot = new NetRobot;
if (netRobot->isConnectionOK() == true)
{
cout << "connect is success!" << endl;
}
// vector<double> commands = {12.0, 12.0, 12.0, 12.0, 12.0, 12.0};
// vector<double> commands = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1};
vector<double> commands = {12.3, 12.3, 12.3, 12.3, 12.3, 12.3};
// vector<double> commands = {123.0, 123.0, 123.0, 123.0, 123.0, 123.0};
// vector<double> commands = {120, 120, 120, 120, 120, 120};
while (1)
{
// 接受
netRobot->socketRecvRun();
netRobot->parseCommand();
// 发送
netRobot->socketSendRun(commands, 0);
usleep(1000); // 微秒
}
// netRobot->socketRecvRun();
// netRobot->parseCommand();
// // 发送
// netRobot->socketSendRun(commands, 0);
// close(netRobot->new_fd_);
return 0;
}
- 客户端
#include <iostream>
#include <vector>
#include <string.h>
#include <thread>
#include <chrono>
#include <sstream>
#include <stdio.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include <fstream>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/time.h>
#include <math.h>
#include <iomanip>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
using std::cout;
using std::endl;
// using std::shared_ptr;
using std::string;
using std::vector;
using namespace std;
// using namespace std::chrono;
using std::perror;
// #define _SERVER_ADDRESS_ "127.0.0.1"
#define _SERVER_ADDRESS_ "192.168.56.3"
#define _SERVER_PORT_ 9054 // 9054
#define _BUFFER_SIZE_ 2048 // 70 1024
class NetClient
{
public:
NetClient();
~NetClient();
void mainLoop();
int socketRecvRun();
// int socketSendRun();
int socketSendRun(vector<double> &commands, int flag);
bool isConnectionOK();
int initSocketClient();
bool tryToConnection();
void parseCommand();
vector<double> eepose_cmd = vector<double>(6, 0.0);
bool is_robotInit_ok_ = false;
struct timeval tv;
int recv_counts = 0;
int send_counts = 0;
ofstream outfile1;
bool data_flag = true;
/* 在sock_fd上进行监听，new_fd 接受新的连接 */
int sock_fd_, new_fd_;
/* 自己的地址信息 */
struct sockaddr_in my_addr_;
/* 连接者的地址信息*/
struct sockaddr_in their_addr_;
int sin_size_;
/* 从buffer中读取的位数 */
int buf_bytes_;
/* buffer */
char buf_[_BUFFER_SIZE_];
char buf_send_[_BUFFER_SIZE_];
char buf_xy_[_BUFFER_SIZE_];
bool is_connection_ok_;
};
NetClient::NetClient()
{
// sin_size_ = sizeof(struct sockaddr_in);
/*
create ......
*/
}
NetClient::~NetClient()
{
// outfile1.close();
/*
delete ......
*/
}
int NetClient::initSocketClient()
{
// outfile1 = ofstream("/home/vision/Documents/socket_data/client_data_01.txt", ios::trunc);
// 1.创建一个socket
sock_fd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sock_fd_ == -1)
{
std::cout << "create client socket error." << std::endl;
return -1;
}
#if 1
int socket_buf_size = 8192; // 16KB：16384； 8KB：8192
setsockopt(sock_fd_, SOL_SOCKET, SO_SNDBUF, (char *)&socket_buf_size, sizeof(socket_buf_size));
setsockopt(sock_fd_, SOL_SOCKET, SO_RCVBUF, (char *)&socket_buf_size, sizeof(socket_buf_size));
#endif
#if 0
int flag = 1;
if ( setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(flag)) == -1 )
{
cout << "couldn't setsockopt(TCP_NODELAY)" << endl;
return -1;
}
#endif
// 2.连接服务器
// ROS_INFO("Try to connect server.");
cout << "Try to connect server." << endl;
my_addr_.sin_family = AF_INET;
my_addr_.sin_addr.s_addr = inet_addr(_SERVER_ADDRESS_);
my_addr_.sin_port = htons(_SERVER_PORT_);
if (connect(sock_fd_, (struct sockaddr *)&my_addr_, sizeof(my_addr_)) == -1)
{
std::cout << "connect socket error." << std::endl;
close(sock_fd_);
return -1;
}
// ROS_INFO("Socket client initialized.");
cout << "Socket client initialized." << endl;
return 0;
}
bool NetClient::isConnectionOK()
{
if (initSocketClient()) // 初始化socket，监听连接请求
return false; // 初始化有问题
is_connection_ok_ = tryToConnection(); // 等待连接请求
// close(sock_fd_); // 建立连接后，关闭监听socket
return is_connection_ok_; // 初始化过程已完成
}
bool NetClient::tryToConnection()
{
//连接成功以后，我们再将 sock_fd_ 设置成非阻塞模式，
//不能在创建时就设置，这样会影响到 connect 函数的行为
#if 1
int oldSocketFlag = fcntl(sock_fd_, F_GETFL, 0);
int newSocketFlag = oldSocketFlag | O_NONBLOCK;
if (fcntl(sock_fd_, F_SETFL, newSocketFlag) == -1)
{
close(sock_fd_);
std::cout << "set socket to nonblock error." << std::endl;
return false;
}
#endif
return true;
}
// 对buf_接收到的指令进行解析
void NetClient::parseCommand()
{
if (data_flag == false)
{
data_flag = true;
return;
}
// cout << "!!! recv_counts: " << recv_counts << endl;
// cout << *(double* )buf_ << endl;
// if (buf_[0] == 'A') // 解析1：末端位姿指令 平移量+旋转量 旋转量用四元数还是什么表示呢？
if (fabs(*(double* )buf_ - 1111.1111) < 1e-4)
{
double *pPose = (double *)(buf_ + 8);
for (int i = 0; i < 6; i++)
{
eepose_cmd[i] = pPose[i];
}
// cout << "yyds..." << endl;
// outfile1 << recv_counts << " " << eepose_cmd[0] << " " << eepose_cmd[1] << " "
// << eepose_cmd[2] << " " << eepose_cmd[3] << " "
// << eepose_cmd[4] << " " << eepose_cmd[5] << endl;
// for (int i = 0; i < 6; i++)
// {
// cout << eepose_cmd[i] << " ";
// }
// cout << endl;
}
recv_counts++;
}
int NetClient::socketRecvRun()
{
memset(buf_, 0, _BUFFER_SIZE_);
buf_bytes_ = recv(sock_fd_, buf_, _BUFFER_SIZE_, 0);
/* 客户端连接关闭，结束连接，等待新的连接请求 */
if (buf_bytes_ == 0)
{
// ROS_WARN("Socket server has disconnected, close current connection.");
std::cout << "Socket server has disconnected, close current connection." << std::endl;
data_flag = false;
close(sock_fd_);
is_connection_ok_ = false;
return 0;
}
/* 接收异常，结束连接，等待新的连接请求 */
if (buf_bytes_ < 0)
{
if (errno == EWOULDBLOCK)
{
data_flag = false;
// 内核缓冲区没有数据了
// cout << "there is no data available now." << endl;
// outfile1 << "111111" << endl;
}
else if (errno == EINTR)
{
data_flag = false;
// 被信号中断了，需要重新尝试发送
// cout << "recv data interrupted by signal." << endl;
// outfile1 << "222222" << endl;
}
else
{
// 出现了严重错误
// cout << "Socket receiving error, close current connection." << endl;
data_flag = false;
close(sock_fd_);
is_connection_ok_ = false;
return 0;
}
}
/* 接收正常，将buffer最后一位的\n改成\0 */
if (buf_bytes_ > 0)
{
// 视情况判断这部分是否保留
#if 0
if (buf_[buf_bytes_ - 1] == '\n')
{
buf_[buf_bytes_ - 1] = '\0';
buf_bytes_--;
}
#endif
// ROS_INFO("Received command: %s\n", buf_);
// cout << "Received data: ";
// for (int i = 0; i < 6; i++)
// {
// cout << *(double* )(buf_+8*i) << " ";
// }
// cout << endl;
}
return 1;
}
int NetClient::socketSendRun(vector<double> &commands, int flag)
{
// 要发送什么东西，先存在buf_send_中！
memset(buf_send_, 0, _BUFFER_SIZE_);
// double *pData = (double *)(buf_send_ + 8);
double *pData = (double *)buf_send_;
if (flag == 0) // 指令1：所有关节位置
{
pData[0] = 1111.1111; // 前8字节：标志位
for (int i = 0; i < 6; i++) // 9-56字节：1-6关节角度值；
{
pData[i+1] = commands[i];
}
}
else if (flag == 1) // 指令2：所有关节速度
{
pData[0] = 2222.2222; // 前8字节：标志位
for (int i = 0; i < 6; i++) // 9-56字节：1-6关节速度值；
{
pData[i+1] = commands[i];
}
}
else if (flag == 2) // 指令3：所有关节电流
{
// for (int i = 0; i < 6; i++)
// {
// cout << setiosflags(ios::fixed) << setprecision(6) << commands[i] << " ";
// }
// cout << endl;
pData[0] = 3333.3333; // 前8字节：标志位
for (int i = 0; i < 6; i++) // 9-56字节：1-6关节电流值;
{
// if (i == 0)
// {
// pData[i+1] = send_counts;
// continue;
// }
pData[i+1] = commands[i];
}
}
else if (flag == 3) // 指令4：机械臂已就位
{
pData[0] = 4444.4444;
}
else if (flag == 4) // 指令5：末端位姿：平移+旋转 旋转量用四元数还是什么呢
{
pData[0] = 5555.5555;
for (int i = 0; i < 6; i++) // 平移+旋转
{
pData[i+1] = commands[i];
}
}
// cout << strlen(buf_send_) << endl;
// int ret = send(sock_fd_, buf_send_, strlen(buf_send_), 0);
// gettimeofday(&tv, NULL);
// printf("millisecond: %ld\n", tv.tv_sec * 1000 + tv.tv_usec / 1000); // 毫秒
// printf("microsecond: %ld\n", tv.tv_sec * 1000000 + tv.tv_usec); // 徽秒
int ret = send(sock_fd_, buf_send_, _BUFFER_SIZE_, 0);
if (ret == -1)
{
if (errno == EWOULDBLOCK)
{
// 非阻塞模式下send函数由于TCP窗口太小发不出去数据
// cout << "send data error as TCP window size is too small." << endl;
// 需要判别是否需要发送！！！
return 0;
}
else if (errno == EINTR)
{
// 被信号中断
// cout << "sending data interrupted by signal." << endl;
// 需要判别是否需要发送！！！
return 0;
}
else
{
// gettimeofday(&tv, NULL);
// printf("millisecond: %ld\n", tv.tv_sec * 1000 + tv.tv_usec / 1000); // 毫秒
// printf("microsecond: %ld\n", tv.tv_sec * 1000000 + tv.tv_usec); // 徽秒
// cout << "errno: " << errno << endl;
// 出现严重错误！！
// cout << "send data error." << endl;
close(sock_fd_);
return -1;
}
}
else if (ret == 0)
{
// 客户端关闭了连接
// cout << "the server down, send data error." << endl;
close(sock_fd_);
return -1;
}
// cout << "send the msg to server successfully!" << endl;
// cout << "!!! send_counts: " << send_counts << endl;
send_counts++;
return 0;
}
int main(int argc, char *argv[])
{
NetClient* netClient = new NetClient;
if (netClient->isConnectionOK() == true)
{
cout << "connect is success!" << endl;
}
vector<double> eepose = {123.4056, 0.0012, 5.0321, 25.06397, 6.08924, 0.0030};
// vector<double> commands = {123, 123.0, 123.0, 123.0, 123.0, 123.0};
while (1)
{
// 接受
netClient->socketRecvRun();
netClient->parseCommand();
// 发送
netClient->socketSendRun(eepose, 4);
usleep(1000); // 微秒
}
// gettimeofday(&netClient->tv, NULL);
// printf("millisecond: %ld\n", netClient->tv.tv_sec * 1000 + netClient->tv.tv_usec / 1000); // 毫秒
// printf("microsecond: %ld\n", netClient->tv.tv_sec * 1000000 + netClient->tv.tv_usec); // 徽秒
// netClient->socketSendRun(joint_current, 2);
// netClient->socketRecvRun();
// netClient->parseCommand();
// gettimeofday(&netClient->tv, NULL);
// printf("millisecond: %ld\n", netClient->tv.tv_sec * 1000 + netClient->tv.tv_usec / 1000); // 毫秒
// printf("microsecond: %ld\n", netClient->tv.tv_sec * 1000000 + netClient->tv.tv_usec); // 徽秒
// close(netClient->sock_fd_);
return 0;
}
扫一扫
528
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
