/**
* @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);
}
}
}
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);
}