コード例 #1
0
/**
 * @brief Destructor.
 * Stop serving images and destroy this class.
 */
PCVideoServer::~PCVideoServer()
{
    // Stop the images to PC server.
    Stop();
    // Clear the error so that you can use this object to make a connection to
    // the VIDEO_TO_PC_PORT to stop the ImageToPCServer if it is waiting to
    // accept connections from a PC.
    ClearError();
    // Open a socket.
    int camSock = socket(AF_INET, SOCK_STREAM, 0);
    if (camSock == ERROR)
    {
	wpi_setErrnoError();
	return;
    }
    ScopedSocket scopedCamSock(camSock);
    // If successful
    if (!StatusIsFatal())
    {
	//  Create a connection to the localhost.
	struct sockaddr_in selfAddr;
	int sockAddrSize = sizeof(selfAddr);
	bzero ((char *) &selfAddr, sockAddrSize);
	selfAddr.sin_family = AF_INET;
#if defined(__FreeBSD__) || defined(NETBSD)
	selfAddr.sin_len = (u_char) sockAddrSize;
#endif
	selfAddr.sin_port = htons (VIDEO_TO_PC_PORT);

	if (( (int)(selfAddr.sin_addr.s_addr = inet_addr (const_cast<char*>("localhost")) ) != ERROR) ||
	    ( (int)(selfAddr.sin_addr.s_addr = hostGetByName (const_cast<char*>("localhost")) ) != ERROR))
	{
	    struct timeval connectTimeout;
	    connectTimeout.tv_sec = 1;
	    connectTimeout.tv_usec = 0;
	    connectWithTimeout(camSock, (struct sockaddr *) &selfAddr, sockAddrSize, &connectTimeout);
	}
    }
}
コード例 #2
0
void CameraServer::Serve() {
  int sock = socket(AF_INET, SOCK_STREAM, 0);

  if (sock == -1)
    wpi_setErrnoError();

  int reuseAddr = 1;
  if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr, sizeof(reuseAddr)) == -1)
    wpi_setErrnoError();

  sockaddr_in address, clientAddress;

  memset(&address, 0, sizeof(address));
  address.sin_family = AF_INET;
  address.sin_addr.s_addr = htonl(INADDR_ANY);
  address.sin_port = htons(kPort);

  if (bind(sock, (struct sockaddr *)&address, sizeof(address)) == -1)
    wpi_setErrnoError();

  if (listen(sock, 10) == -1)
    wpi_setErrnoError();

  while(true) {
    socklen_t clientAddressLen = sizeof(clientAddress);

    int conn = accept(sock, (struct sockaddr*)&clientAddress, &clientAddressLen);
    if (conn == -1) {
      wpi_setErrnoError();
      continue;
    }

    Request req;
    if (read(conn, &req, sizeof(req)) == -1) {
      wpi_setErrnoError();
      close(conn);
      continue;
    } else {
      req.fps = ntohl(req.fps);
      req.compression = ntohl(req.compression);
      req.size = ntohl(req.size);
    }

    // TODO: Support the SW Compression. The rest of the code below will work as though this
    // check isn't here
    if (req.compression != kHardwareCompression) {
      wpi_setWPIErrorWithContext(IncompatibleState, "Choose \"USB Camera HW\" on the dashboard");
      close(conn);
      continue;
    }

    {
      // Wait for the camera to be setw
      std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
      if (!m_camera) {
        std::cout << "Camera not yet ready, awaiting first image" << std::endl;
        m_newImageVariable.wait(lock);
      }
      m_hwClient = req.compression == kHardwareCompression;
      if (!m_hwClient) SetQuality(100 - req.compression);
      else if (m_camera) m_camera->SetFPS(req.fps);
      SetSize(req.size);
    }

    auto period = std::chrono::microseconds(1000000) / req.fps;
    while (true) {
      auto startTime = std::chrono::steady_clock::now();
      std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData;
      {
        std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
        m_newImageVariable.wait(lock);
        imageData = m_imageData;
        m_imageData = std::make_tuple(nullptr, 0, 0, false);
      }

      unsigned int size = std::get<1>(imageData);
      unsigned int netSize = htonl(size);
      unsigned int start = std::get<2>(imageData);
      uint8_t *data = std::get<0>(imageData);

      if (data == nullptr) continue;

      if (write(conn, kMagicNumber, sizeof(kMagicNumber)) == -1) {
        wpi_setErrnoErrorWithContext("[CameraServer] Error sending magic number");
        FreeImageData(imageData);
        break;
      }
      if (write(conn, &netSize, sizeof(netSize)) == -1) {
        wpi_setErrnoErrorWithContext("[CameraServer] Error sending image size");
        FreeImageData(imageData);
        break;
      }
      if (write(conn, &data[start], sizeof(uint8_t) * size) == -1) {
        wpi_setErrnoErrorWithContext("[CameraServer] Error sending image data");
        FreeImageData(imageData);
        break;
      }
      FreeImageData(imageData);
      std::this_thread::sleep_until(startTime + period);
    }
    close(conn);
  }
  close(sock);
}