pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/physics/TcpCommunication.cc

/*
 * Copyright (C) 2019 AIRC 01
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: A TCP communication to send and receive Pose informations of robot entities
 * Author: Zhang Shuai & Zeng Lei
 */

#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
// #include <boost/algorithm/string.hpp>

// #include "gazebo/physics/World.hh"

#include "gazebo/physics/TcpCommunication.hh"

using namespace gazebo;
using namespace physics;

/////////////////////////////////////////////////
TcpServer::TcpServer() : client_fd(-1)
{
  server_fd = socket(AF_INET, SOCK_STREAM, 0);
  memset(&server_sockaddr, 0, sizeof(struct sockaddr_in));
  memset(&client_sockaddr, 0, sizeof(struct sockaddr_in));
}

/////////////////////////////////////////////////
TcpServer::~TcpServer()
{
  if (-1 != client_fd)
    close(client_fd);
  close(server_fd);
}

/////////////////////////////////////////////////
void TcpServer::StartServer()
{
  printf("Start to Tcp Server!\n");
  server_sockaddr.sin_family = AF_INET;
  server_sockaddr.sin_port = htons(port);
  server_sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
  unsigned int value = 0x1;
  setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, (void *)&value, sizeof(value));
  if (bind(server_fd, (struct sockaddr *)&server_sockaddr, sizeof(server_sockaddr)) == -1)
  {
    perror("bind\n");
    return;
  }
  if (listen(server_fd, QUEUE) == -1)
  {
    perror("listen\n");
    return;
  }

  socklen_t length = sizeof(client_sockaddr);
  
  client_fd = accept(server_fd, (struct sockaddr *)&client_sockaddr, &length);
  if (client_fd < 0)
  {
    perror("connect\n");
    return;
  }
  printf("A client connected Succeed!\n");
}

/////////////////////////////////////////////////
bool TcpServer::sendData(uint64_t nStep)
{
  memset(buff, 0, BUF_SIZE);
  memcpy(buff, &nStep, sizeof(uint64_t));
  // std::cout<<"send data: "<<nStep<<std::endl;
  if (send(client_fd, buff, sizeof(uint64_t), 0) > 0)
  {
    // std::cout<<"Send Data Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Send Data Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
bool TcpServer::recvData(uint64_t &nStep)
{
  memset(buff, 0, BUF_SIZE);
  // std::cout<<"recv data: "<<std::endl;
  if (recv(client_fd, buff, sizeof(uint64_t), 0) > 0)
  {
    memcpy(&nStep, buff, sizeof(uint64_t));
    // std::cout<<"Recv Data "<< nStep<<"Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Recv Data Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
bool TcpServer::sendInfor(CommunicationData *_infor, unsigned int _simula_entity_num)
{
  memset(buff, 0, BUF_SIZE);
  memcpy(buff, (char *)_infor, sizeof(CommunicationData) * _simula_entity_num);
  // std::cout<<"send data: "<< infor <<std::endl;
  if (send(client_fd, buff, sizeof(CommunicationData) * _simula_entity_num, 0) > 0)
  {
    // std::cout<<"Send Infor Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Send Infor Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
bool TcpServer::recvInfor(CommunicationData *_infor, unsigned int _shadow_entity_num)
{
  memset(buff, 0, BUF_SIZE);
  // std::cout<<"recv data: "<<std::endl;
  if (recv(client_fd, buff, sizeof(CommunicationData) * _shadow_entity_num, 0) > 0)
  {
    memcpy((char *)_infor, buff, sizeof(CommunicationData) * _shadow_entity_num);
    // std::cout<<"Recv Infor " << infor << "Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Recv Infor Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
void TcpServer::SetPort(int _port)
{
  port = _port;
}

/////////////////////////////////////////////////
void TcpServer::Close()
{
  close(client_fd);
  close(server_fd);
}

/////////////////////////////////////////////////
/////////////////////////////////////////////////
TcpClient::TcpClient()
{
  conn_fd = socket(AF_INET, SOCK_STREAM, 0);
  memset(&server_sockaddr, 0, sizeof(server_sockaddr));
}

/////////////////////////////////////////////////
TcpClient::~TcpClient()
{
  close(conn_fd);
}

/////////////////////////////////////////////////
void TcpClient::ConnectServer()
{
  server_sockaddr.sin_family = AF_INET;
  server_sockaddr.sin_port = htons(port);                  
  server_sockaddr.sin_addr.s_addr = inet_addr(ip.c_str()); 
  while (1)
  {
    if (connect(conn_fd, (struct sockaddr *)&server_sockaddr, sizeof(server_sockaddr)) >= 0)
    {
      break;
    }
    perror("connect error!\n");
  } 
}

/////////////////////////////////////////////////
bool TcpClient::sendData(uint64_t nStep)
{
  memset(buff, 0, BUF_SIZE);
  // *((int*)buff) = nStep;
  memcpy(buff, &nStep, sizeof(uint64_t));
  // std::cout<<"send data: "<<nStep<<std::endl;
  if (send(conn_fd, buff, sizeof(uint64_t), 0) > 0)
  {
    // std::cout<<"Send Data Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Send Data Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
bool TcpClient::recvData(uint64_t &nStep)
{
  memset(buff, 0, BUF_SIZE);
  // std::cout<<"recv data: "<<std::endl;
  if (recv(conn_fd, buff, sizeof(uint64_t), 0) > 0)
  {
    memcpy(&nStep, buff, sizeof(uint64_t));
    // std::cout<<"Recv Data "<< nStep<<"Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Recv Data Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
bool TcpClient::sendInfor(CommunicationData *_infor, unsigned int _simula_entity_num)
{
  memset(buff, 0, BUF_SIZE);
  memcpy(buff, (char *)_infor, sizeof(CommunicationData) * _simula_entity_num);
  // std::cout<<"send data: "<< infor <<std::endl;
  if (send(conn_fd, buff, sizeof(CommunicationData) * _simula_entity_num, 0) > 0)
  {
    // std::cout<<"Send Infor Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Send Infor Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
bool TcpClient::recvInfor(CommunicationData *_infor, unsigned int _shadow_entity_num)
{
  memset(buff, 0, BUF_SIZE);
  // std::cout<<"recv data: "<<std::endl;
  if (recv(conn_fd, buff, sizeof(CommunicationData) * _shadow_entity_num, 0) > 0)
  {
    memcpy((char *)_infor, buff, sizeof(CommunicationData) * _shadow_entity_num);
    // std::cout<<"Recv Infor " << infor << "Succeed!"<<std::endl;
    return true;
  }
  else
  {
    // std::cout<<"Recv Infor Failed!"<<std::endl;
    return false;
  }
}

/////////////////////////////////////////////////
void TcpClient::SetIp(std::string _ip)
{
  ip = _ip;
}

/////////////////////////////////////////////////
void TcpClient::SetPort(int _port)
{
  port = _port;
}

/////////////////////////////////////////////////
void TcpClient::Close()
{
  close(conn_fd);
}