/** * Create a socket connected to camera * Used to create a connection to the camera for both capturing images and * setting parameters. * @param requestString The initial request string to send upon successful * connection. * @param setError If true, rais an error if there's a problem creating the * connection. * This is only enabled after several unsucessful connections, so a single one * doesn't * cause an error message to be printed if it immediately recovers. * @return -1 if failed, socket handle if successful. */ int AxisCamera::CreateCameraSocket(std::string const &requestString, bool setError) { struct addrinfo *address = nullptr; int camSocket; /* create socket */ if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1) { if (setError) wpi_setErrnoErrorWithContext("Failed to create the camera socket"); return -1; } if (getaddrinfo(m_cameraHost.c_str(), "80", nullptr, &address) == -1) { if (setError) { wpi_setErrnoErrorWithContext("Failed to create the camera socket"); close(camSocket); } return -1; } /* connect to server */ if (connect(camSocket, address->ai_addr, address->ai_addrlen) == -1) { if (setError) wpi_setErrnoErrorWithContext("Failed to connect to the camera"); freeaddrinfo(address); close(camSocket); return -1; } freeaddrinfo(address); int sent = send(camSocket, requestString.c_str(), requestString.size(), 0); if (sent == -1) { if (setError) wpi_setErrnoErrorWithContext("Failed to send a request to the camera"); close(camSocket); return -1; } return camSocket; }
/** * Send a request to the camera to set all of the parameters. This is called * in the capture thread between each frame. This strategy avoids making lots * of redundant HTTP requests, accounts for failed initial requests, and * avoids blocking calls in the main thread unless necessary. * * This method does nothing if no parameters have been modified since it last * completely successfully. * * @return <code>true</code> if the stream should be restarted due to a * parameter changing. */ bool AxisCamera::WriteParameters() { if (m_parametersDirty) { std::stringstream request; request << "GET /axis-cgi/admin/param.cgi?action=update"; m_parametersMutex.lock(); request << "&ImageSource.I0.Sensor.Brightness=" << m_brightness; request << "&ImageSource.I0.Sensor.WhiteBalance=" << kWhiteBalanceStrings[m_whiteBalance]; request << "&ImageSource.I0.Sensor.ColorLevel=" << m_colorLevel; request << "&ImageSource.I0.Sensor.Exposure=" << kExposureControlStrings[m_exposureControl]; request << "&ImageSource.I0.Sensor.ExposurePriority=" << m_exposurePriority; request << "&Image.I0.Stream.FPS=" << m_maxFPS; request << "&Image.I0.Appearance.Resolution=" << kResolutionStrings[m_resolution]; request << "&Image.I0.Appearance.Compression=" << m_compression; request << "&Image.I0.Appearance.Rotation=" << kRotationStrings[m_rotation]; m_parametersMutex.unlock(); request << " HTTP/1.1" << std::endl; request << "User-Agent: HTTPStreamClient" << std::endl; request << "Connection: Keep-Alive" << std::endl; request << "Cache-Control: no-cache" << std::endl; request << "Authorization: Basic RlJDOkZSQw==" << std::endl; request << std::endl; int socket = CreateCameraSocket(request.str(), false); if (socket == -1) { wpi_setErrnoErrorWithContext("Error setting camera parameters"); } else { close(socket); m_parametersDirty = false; if (m_streamDirty) { m_streamDirty = false; return true; } } } return false; }
/** * @brief Initialize the socket and serve images to the PC. * This is the task that serves images to the PC in a loop. This runs * as a separate task. */ int PCVideoServer::ServerTask() { /* Setup to PC sockets */ struct sockaddr_in serverAddr; int sockAddrSize = sizeof(serverAddr); int pcSock = ERROR; bzero ((char *) &serverAddr, sockAddrSize); serverAddr.sin_len = (u_char) sockAddrSize; serverAddr.sin_family = AF_INET; serverAddr.sin_port = htons (VIDEO_TO_PC_PORT); serverAddr.sin_addr.s_addr = htonl (INADDR_ANY); int success; while (true) { taskSafe(); // Create the socket. if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR) { wpi_setErrnoErrorWithContext("Failed to create the PCVideoServer socket"); continue; } // Set the TCP socket so that it can be reused if it is in the wait state. int reuseAddr = 1; setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr)); // Bind socket to local address. if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR) { wpi_setErrnoErrorWithContext("Failed to bind the PCVideoServer port"); close (pcSock); continue; } // Create queue for client connection requests. if (listen (pcSock, 1) == ERROR) { wpi_setErrnoErrorWithContext("Failed to listen on the PCVideoServer port"); close (pcSock); continue; } struct sockaddr_in clientAddr; int clientAddrSize; int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize); if (newPCSock == ERROR) { close(pcSock); continue; } //TODO: check camera error int numBytes = 0; int imageDataSize = 0; char* imageData = NULL; while(!m_stopServer) { success = semTake(m_newImageSem, 1000); if (success == ERROR) { // If the semTake timed out, there are no new images from the camera. continue; } success = AxisCamera::GetInstance().CopyJPEG(&imageData, numBytes, imageDataSize); if (!success) { // No point in running too fast - Wait(1.0); // If camera is not initialzed you will get failure and // the timestamp is invalid. Reset this value and try again. continue; } // Write header to PC static const char header[4]={1,0,0,0}; int headerSend = write(newPCSock, const_cast<char*>(header), 4); // Write image length to PC int lengthSend = write(newPCSock, reinterpret_cast<char*>(&numBytes), 4); // Write image to PC int sent = write (newPCSock, imageData, numBytes); // The PC probably closed connection. Get out of here // and try listening again. if (headerSend == ERROR || lengthSend == ERROR || sent == ERROR) { break; } } // Clean up delete [] imageData; close (newPCSock); newPCSock = ERROR; close (pcSock); pcSock = ERROR; taskUnsafe(); Wait(0.1); } return (OK); }
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); }
/** * The internal method to read from a file. * This will be called in its own thread when the preferences singleton is * first created. */ void Preferences::ReadTaskRun() { std::unique_lock<priority_recursive_mutex> sync(m_tableLock); m_fileOpStarted.give(); std::string comment; FILE *file = nullptr; file = fopen(kFileName, "r"); if (file != nullptr) { std::string buffer; while (true) { char value; do { value = fgetc(file); } while (value == ' ' || value == '\t'); if (value == '\n' || value == ';') { if (value == '\n') { comment += "\n"; } else { buffer.clear(); for (; value != '\n' && !feof(file); value = fgetc(file)) buffer += value; buffer += '\n'; comment += buffer; } } else if (value == '[') { // Find the end of the section and the new line after it and throw it // away for (; value != ']' && !feof(file); value = fgetc(file)) ; for (; value != '\n' && !feof(file); value = fgetc(file)) ; } else { buffer.clear(); for (; value != '=' && !feof(file);) { buffer += value; do { value = fgetc(file); } while (value == ' ' || value == '\t'); } std::string name = buffer; buffer.clear(); bool shouldBreak = false; do { value = fgetc(file); } while (value == ' ' || value == '\t'); if (value == '"') { for (value = fgetc(file); value != '"' && !feof(file); value = fgetc(file)) buffer += value; // Clear the line while (fgetc(file) != '\n' && !feof(file)) ; } else { for (; value != '\n' && !feof(file);) { buffer += value; do { value = fgetc(file); } while (value == ' ' || value == '\t'); } if (feof(file)) shouldBreak = true; } std::string value = buffer; if (!name.empty() && !value.empty()) { m_keys.push_back(name); m_values.insert(std::pair<std::string, std::string>(name, value)); NetworkTable::GetTable(kTableName)->PutString(name, value); if (!comment.empty()) { m_comments.insert( std::pair<std::string, std::string>(name, comment)); comment.clear(); } } if (shouldBreak) break; } } } else { wpi_setErrnoErrorWithContext("Opening preferences file"); } if (file != nullptr) fclose(file); if (!comment.empty()) m_endComment = comment; NetworkTable::GetTable(kTableName)->PutBoolean(kSaveField, false); NetworkTable::GetTable(kTableName)->AddTableListener(this); }
/** * This function actually reads the images from the camera. */ void AxisCamera::ReadImagesFromCamera() { char *imgBuffer = nullptr; int imgBufferLength = 0; // TODO: these recv calls must be non-blocking. Otherwise if the camera // fails during a read, the code hangs and never retries when the camera comes // back up. int counter = 2; while (!m_done) { char initialReadBuffer[kMaxPacketSize] = ""; char intermediateBuffer[1]; char *trailingPtr = initialReadBuffer; int trailingCounter = 0; while (counter) { // TODO: fix me... this cannot be the most efficient way to approach this, // reading one byte at a time. if (recv(m_cameraSocket, intermediateBuffer, 1, 0) == -1) { wpi_setErrnoErrorWithContext("Failed to read image header"); close(m_cameraSocket); return; } strncat(initialReadBuffer, intermediateBuffer, 1); // trailingCounter ensures that we start looking for the 4 byte string // after // there is at least 4 bytes total. Kind of obscure. // look for 2 blank lines (\r\n) if (nullptr != strstr(trailingPtr, "\r\n\r\n")) { --counter; } if (++trailingCounter >= 4) { trailingPtr++; } } counter = 1; char *contentLength = strstr(initialReadBuffer, "Content-Length: "); if (contentLength == nullptr) { wpi_setWPIErrorWithContext(IncompatibleMode, "No content-length token found in packet"); close(m_cameraSocket); if (imgBuffer) delete[] imgBuffer; return; } contentLength = contentLength + 16; // skip past "content length" int readLength = atol(contentLength); // get the image byte count // Make sure buffer is large enough if (imgBufferLength < readLength) { if (imgBuffer) delete[] imgBuffer; imgBufferLength = readLength + kImageBufferAllocationIncrement; imgBuffer = new char[imgBufferLength]; if (imgBuffer == nullptr) { imgBufferLength = 0; continue; } } // Read the image data for "Content-Length" bytes int bytesRead = 0; int remaining = readLength; while (bytesRead < readLength) { int bytesThisRecv = recv(m_cameraSocket, &imgBuffer[bytesRead], remaining, 0); bytesRead += bytesThisRecv; remaining -= bytesThisRecv; } // Update image { std::lock_guard<priority_mutex> lock(m_imageDataMutex); m_imageData.assign(imgBuffer, imgBuffer + imgBufferLength); m_freshImage = true; } if (WriteParameters()) { break; } } close(m_cameraSocket); }