Merge pull request #80 from wml-frc/image_stream

MJPEG Image stream

Core/common/libs/MJPEGWriter/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2017 J. Pery
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Core/common/libs/MJPEGWriter/main/cpp/MJPEGWriter.cpp

@@ -0,0 +1,131 @@
+#include "MJPEGWriter.h"
+#include <fstream>
+void
+MJPEGWriter::Listener()
+{
+
+	// send http header
+		std::string header;
+		header += "HTTP/1.0 200 OK\r\n";
+		header += "Cache-Control: no-cache\r\n";
+		header += "Pragma: no-cache\r\n";
+		header += "Connection: close\r\n";
+		header += "Content-Type: multipart/x-mixed-replace; boundary=mjpegstream\r\n\r\n";
+		const int header_size = header.size();
+		char* header_data = (char*)header.data();
+		fd_set rread;
+		SOCKET maxfd;
+		this->open();
+		pthread_mutex_unlock(&mutex_writer);
+		while (true)
+		{
+				rread = master;
+				struct timeval to = { 0, timeout };
+				maxfd = sock + 1;
+				if (sock == INVALID_SOCKET){
+					return;
+				}
+				int sel = select(maxfd, &rread, NULL, NULL, &to);
+				if (sel > 0) {
+						for (int s = 0; s < maxfd; s++)
+						{
+								if (FD_ISSET(s, &rread) && s == sock)
+								{
+										int         addrlen = sizeof(SOCKADDR);
+										SOCKADDR_IN address = { 0 };
+										SOCKET      client = accept(sock, (SOCKADDR*)&address, (socklen_t*)&addrlen);
+										if (client == SOCKET_ERROR)
+										{
+												CJ_CORE_PRINT_ERROR("Error. Couldn't accept connection on sock: " + std::to_string(sock));
+												return;
+										}
+										maxfd = (maxfd>client ? maxfd : client);
+										pthread_mutex_lock(&mutex_cout);
+										CJ_CORE_PRINT_INFO("New Client" + std::to_string(client));
+										char headers[4096] = "\0";
+										int readBytes = _read(client, headers);
+										CJ_CORE_PRINT_TRACE(headers);
+										pthread_mutex_unlock(&mutex_cout);
+										pthread_mutex_lock(&mutex_client);
+										_write(client, header_data, header_size);
+										clients.push_back(client);
+										pthread_mutex_unlock(&mutex_client);
+								}
+						}
+				}
+				usleep(1000);
+		}
+}
+
+void
+MJPEGWriter::Writer()
+{
+		pthread_mutex_lock(&mutex_writer);
+		pthread_mutex_unlock(&mutex_writer);
+		const int milis2wait = 16666;
+		while (this->isOpened())
+		{
+				pthread_mutex_lock(&mutex_client);
+				int num_connected_clients = clients.size();
+				pthread_mutex_unlock(&mutex_client);
+				if (!num_connected_clients) {
+						usleep(milis2wait);
+						continue;
+				}
+				pthread_t threads[NUM_CONNECTIONS];
+				int count = 0;
+
+				std::vector<uchar> outbuf;
+				std::vector<int> params;
+				params.push_back(cv::IMWRITE_JPEG_QUALITY);
+				params.push_back(quality);
+				pthread_mutex_lock(&mutex_writer);
+				imencode(".jpg", lastFrame, outbuf, params);
+				pthread_mutex_unlock(&mutex_writer);
+				int outlen = outbuf.size();
+
+				pthread_mutex_lock(&mutex_client);
+				std::vector<int>::iterator begin = clients.begin();
+				std::vector<int>::iterator end = clients.end();
+				pthread_mutex_unlock(&mutex_client);
+				std::vector<clientPayload*> payloads;
+				for (std::vector<int>::iterator it = begin; it != end; ++it, ++count)
+				{
+						if (count > NUM_CONNECTIONS)
+								break;
+						struct clientPayload *cp = new clientPayload({ (MJPEGWriter*)this, { outbuf.data(), outlen, *it } });
+						payloads.push_back(cp);
+						pthread_create(&threads[count], NULL, &MJPEGWriter::clientWrite_Helper, cp);
+				}
+				for (; count > 0; count--)
+				{
+						pthread_join(threads[count-1], NULL);
+						delete payloads.at(count-1);
+				}
+				usleep(milis2wait);
+		}
+}
+
+void
+MJPEGWriter::ClientWrite(clientFrame & cf)
+{
+		std::stringstream head;
+		head << "--mjpegstream\r\nContent-Type: image/jpeg\r\nContent-Length: " << cf.outlen << "\r\n\r\n";
+		std::string string_head = head.str();
+		pthread_mutex_lock(&mutex_client);
+		_write(cf.client, (char*) string_head.c_str(), string_head.size());
+		int n = _write(cf.client, (char*)(cf.outbuf), cf.outlen);
+	if (n < cf.outlen)
+	{
+			std::vector<int>::iterator it;
+				it = find (clients.begin(), clients.end(), cf.client);
+				if (it != clients.end())
+				{
+					CJ_CORE_PRINT_ERROR("Kill Client Error: " + std::to_string(cf.client));
+					clients.erase(std::remove(clients.begin(), clients.end(), cf.client));
+							::shutdown(cf.client, 2);
+				}
+	}
+		pthread_mutex_unlock(&mutex_client);
+		pthread_exit(NULL);
+}

Core/common/libs/MJPEGWriter/main/include/MJPEGWriter.h

@@ -0,0 +1,176 @@
+#ifndef MJPEG_WRITER_H
+#define MJPEG_WRITER_H
+
+#define PORT        unsigned short
+#define SOCKET    int
+#define HOSTENT  struct hostent
+#define SOCKADDR    struct sockaddr
+#define SOCKADDR_IN  struct sockaddr_in
+#define ADDRPOINTER  unsigned int*
+#define INVALID_SOCKET -1
+#define SOCKET_ERROR   -1
+#define TIMEOUT_M       200000
+#define NUM_CONNECTIONS 10
+
+#include "common_headers.h"
+#include "UDP_TransferNT.h"
+#include <sys/signal.h>
+#include <pthread.h>
+
+struct clientFrame {
+		uchar* outbuf;
+		int outlen;
+		int client;
+};
+
+struct clientPayload {
+		void* context;
+		clientFrame cf;
+};
+
+class MJPEGWriter{
+		SOCKET sock;
+		fd_set master;
+		int timeout;
+		int quality; // jpeg compression [1..100]
+		std::vector<int> clients;
+		pthread_t thread_listen, thread_write;
+		pthread_mutex_t mutex_client = PTHREAD_MUTEX_INITIALIZER;
+		pthread_mutex_t mutex_cout = PTHREAD_MUTEX_INITIALIZER;
+		pthread_mutex_t mutex_writer = PTHREAD_MUTEX_INITIALIZER;
+		cv::Mat lastFrame;
+		int port;
+
+		int _write(int sock, char *s, int len)
+		{
+				if (len < 1) { len = strlen(s); }
+				{
+					try
+					{
+						int retval = ::send(sock, s, len, 0x4000);
+						return retval;
+					}
+					catch (int e)
+					{
+						CJ_CORE_PRINT_ERROR("An exception occurred. Exception Nr. " + std::to_string(e));
+					}
+				}
+				return -1;
+		}
+
+		int _read(int socket, char* buffer)
+		{
+				int result;
+				result = recv(socket, buffer, 4096, MSG_PEEK);
+				if (result < 0 )
+				{
+						CJ_CORE_PRINT_ERROR("An exception occurred. Exception Nr. " + std::to_string(result));
+						return result;
+				}
+				std::string s = buffer;
+				buffer = (char*) s.substr(0, (int) result).c_str();
+				return result;
+		}
+
+		static void* listen_Helper(void* context)
+		{
+				((MJPEGWriter *)context)->Listener();
+				return NULL;
+		}
+
+		static void* writer_Helper(void* context)
+		{
+				((MJPEGWriter *)context)->Writer();
+				return NULL;
+		}
+
+		static void* clientWrite_Helper(void* payload)
+		{
+				void* ctx = ((clientPayload *)payload)->context;
+				struct clientFrame cf = ((clientPayload *)payload)->cf;
+				((MJPEGWriter *)ctx)->ClientWrite(cf);
+				return NULL;
+		}
+
+public:
+
+		MJPEGWriter(int port = 0)
+				: sock(INVALID_SOCKET)
+				, timeout(TIMEOUT_M)
+				, quality(90)
+	, port(port)
+		{
+				signal(SIGPIPE, SIG_IGN);
+				FD_ZERO(&master);
+				// if (port)
+				//     open(port);
+		}
+
+		~MJPEGWriter()
+		{
+				release();
+		}
+
+		bool release()
+		{
+				if (sock != INVALID_SOCKET)
+						shutdown(sock, 2);
+				sock = (INVALID_SOCKET);
+				return false;
+		}
+
+		bool open()
+		{
+				sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+
+				SOCKADDR_IN address;
+				address.sin_addr.s_addr = INADDR_ANY;
+				address.sin_family = AF_INET;
+				address.sin_port = htons(port);
+				if (::bind(sock, (SOCKADDR*)&address, sizeof(SOCKADDR_IN)) == SOCKET_ERROR)
+				{
+						CJ_CORE_PRINT_ERROR("Error, could not bind to socket: " + std::to_string(sock));
+						return release();
+				}
+				if (listen(sock, NUM_CONNECTIONS) == SOCKET_ERROR)
+				{
+						CJ_CORE_PRINT_ERROR("Error, could not listen on sock: " + std::to_string(sock));
+						return release();
+				}
+				FD_SET(sock, &master);
+				return true;
+		}
+
+		bool isOpened()
+		{
+				return sock != INVALID_SOCKET;
+		}
+
+		void start(){
+				pthread_mutex_lock(&mutex_writer);
+				pthread_create(&thread_listen, NULL, this->listen_Helper, this);
+				pthread_create(&thread_write, NULL, this->writer_Helper, this);
+		}
+
+		void stop(){
+			this->release();
+				pthread_join(thread_listen, NULL);
+				pthread_join(thread_write, NULL);
+		}
+
+		void write(cv::Mat frame){
+			pthread_mutex_lock(&mutex_writer);
+			if(!frame.empty()){
+				lastFrame.release();
+				lastFrame = frame.clone();
+			}
+			pthread_mutex_unlock(&mutex_writer);
+		}
+
+private:
+		void Listener();
+		void Writer();
+		void ClientWrite(clientFrame &cf);
+};
+
+#endif

Core/common/main/include/Platform/PlatformDetection.h

@@ -7,7 +7,7 @@
 #elif defined(__linux__)
 	#define CJ_PLATFORM_LINUX
 
-#elif defined (__APPLE__) || defined(__MACH__)
+#elif defined(__APPLE__) || defined(__MACH__)
 	#include <TargetConditionals.h>
 
 	#if TARGET_IPHONE_SIMULATOR == 1

Core/common/main/include/common_headers.h

@@ -1,6 +1,8 @@
 #ifndef COMMON_HEADERS_H
 #define COMMON_HEADERS_H
 
+#include "Platform/PlatformDetection.h"
+
 // System common headers
 #include <iostream>
 #include <string>

Core/src/latest/Makefile

@@ -65,17 +65,27 @@ UDP_TRANSFERNT_LIB_HEADERS = -I$(UDP_TRANSFERNT_LIB_LOC)/include
 SPDLOG_LIB_LOC = $(COMMON_LOC)/libs/spdlog
 SPDLOG_LIB_HEADERS = -I$(SPDLOG_LIB_LOC)/include
 
+#
+# MJPEGWriter
+#
+MJPEG_WRITER_LOC = $(COMMON_LOC)/libs/MJPEGWriter
+MJPEG_WRITER_SRCS = $(call rwildcard,$(MJPEG_WRITER_LOC)/main/,*.cpp)
+MJPEG_WRITER_SRC_DIR = $(MJPEG_WRITER_LOC)/main/cpp
+
+MJPEG_WRITER_HEADERS = -I$(MJPEG_WRITER_LOC)/main/include
+
 
 # Obejcts from cpp files
 ENTRY_OBJ := $(patsubst $(ENTRY_DIR)/%.cpp, $(OUTDIR)/%.o, $(ENTRY_SRC))
 COPROC_OBJ := $(patsubst $(COPROC_SRC_DIR)/%.cpp, $(OUTDIR)/%.o, $(COPROC_SRCS))
 CJ_VISION_OBJ := $(patsubst $(CJ_VISION_SRC_DIR)/%.cpp, $(OUTDIR)/%.o, $(CJ_VISION_SRCS))
 COMMON_OBJ := $(patsubst $(COMMON_SRC_DIR)/%.cpp, $(OUTDIR)/%.o, $(COMMON_SRCS))
+MJPEG_WRITER_OBJ := $(patsubst $(MJPEG_WRITER_SRC_DIR)/%.cpp, $(OUTDIR)/%.o, $(MJPEG_WRITER_SRCS))
 
 # All
-ALL_HEADERS = $(CJ_VISION_HEADERS) $(COMMON_HEADERS) $(UDP_TRANSFERNT_LIB_HEADERS) $(SPDLOG_LIB_HEADERS) $(COPROC_HEADERS)
-ALL_OBJ = $(ENTRY_OBJ) $(CJ_VISION_OBJ) $(COMMON_OBJ) $(COPROC_OBJ)
-ALL_SRCS = $(ENTRY_SRC) $(CJ_VISION_SRCS) $(COMMON_SRCS) $(COPROC_SRCS)
+ALL_HEADERS = $(CJ_VISION_HEADERS) $(COMMON_HEADERS) $(UDP_TRANSFERNT_LIB_HEADERS) $(SPDLOG_LIB_HEADERS) $(COPROC_HEADERS) $(MJPEG_WRITER_HEADERS)
+ALL_OBJ = $(ENTRY_OBJ) $(CJ_VISION_OBJ) $(COMMON_OBJ) $(COPROC_OBJ) $(MJPEG_WRITER_OBJ)
+ALL_SRCS = $(ENTRY_SRC) $(CJ_VISION_SRCS) $(COMMON_SRCS) $(COPROC_SRCS) $(MJPEG_WRITER_SRCS)
 
 # Compile script
 MK_OUT = mkdir -p $@; rm -rf $@
@@ -120,5 +130,11 @@ $(OUTDIR)/%.o: $(COMMON_SRC_DIR)/%.cpp
 	$(MK_OUT)
 	$(COMPILE_OBJ)
 
+# MJPEG Writer
+$(OUTDIR)/%.o: $(MJPEG_WRITER_SRC_DIR)/%.cpp
+	@echo Building objects
+	$(MK_OUT)
+	$(COMPILE_OBJ)
+
 clean:
 	rm -rf ${OUTDIR}