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