ROVER - Modularization - added new tasks: socket communication, image processing, booth modes
Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/rover/src/tasks/booth_modes_task.cpp b/rover/src/tasks/booth_modes_task.cpp
new file mode 100644
index 0000000..1a09cd7
--- /dev/null
+++ b/rover/src/tasks/booth_modes_task.cpp
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2017 FH Dortmund and others
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ * Booth modes implemented for demonstration of the APP4MC-APPSTACLE Rover
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created 23.10.2017
+ *
+ */
+
+#include <tasks/booth_modes_task.h>
+
+#include <wiringPi.h>
+#include <ctime>
+#include <unistd.h>
+#include <libraries/timing/timing.h>
+#include <api/basic_psys_rover.h>
+#include <interfaces.h>
+#include <pthread.h>
+#include <roverapp.h>
+#include <softPwm.h>
+
+#include <libraries/pthread_monitoring/collect_thread_name.h>
+
+void *Booth_Modes_Task(void * arg)
+{
+ timing booth_task_tmr;
+ booth_task_tmr.setTaskID("BM");
+ booth_task_tmr.setDeadline(0.1);
+ booth_task_tmr.setPeriod(0.1);
+
+ CollectThreadName("Booth_Modes_Task");
+
+ while (1)
+ {
+ booth_task_tmr.recordStartTime();
+ booth_task_tmr.calculatePreviousSlackTime();
+
+ //Task content starts here -----------------------------------------------
+
+ if (driving_mode == BOOTH1)
+ {
+
+ }
+
+ if (driving_mode == BOOTH2)
+ {
+
+ }
+
+ //Task content ends here -------------------------------------------------
+
+ booth_task_tmr.recordEndTime();
+ booth_task_tmr.calculateExecutionTime();
+ booth_task_tmr.calculateDeadlineMissPercentage();
+ booth_task_tmr.incrementTotalCycles();
+ pthread_mutex_lock(&booth_task_ti_l);
+ booth_task_ti.deadline = booth_task_tmr.getDeadline();
+ booth_task_ti.deadline_miss_percentage = booth_task_tmr.getDeadlineMissPercentage();
+ booth_task_ti.execution_time = booth_task_tmr.getExecutionTime();
+ booth_task_ti.period = booth_task_tmr.getPeriod();
+ booth_task_ti.prev_slack_time = booth_task_tmr.getPrevSlackTime();
+ booth_task_ti.task_id = booth_task_tmr.getTaskID();
+ booth_task_ti.start_time = booth_task_tmr.getStartTime();
+ booth_task_ti.end_time = booth_task_tmr.getEndTime();
+ pthread_mutex_unlock(&booth_task_ti_l);
+ booth_task_tmr.sleepToMatchPeriod();
+ }
+
+ /* the function must return something - NULL will do */
+ return NULL;
+}
+
+
+
diff --git a/rover/src/tasks/image_processing_task.cpp b/rover/src/tasks/image_processing_task.cpp
new file mode 100644
index 0000000..08070d2
--- /dev/null
+++ b/rover/src/tasks/image_processing_task.cpp
@@ -0,0 +1,364 @@
+/*
+ * Copyright (c) 2017 FH Dortmund and others
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ * Image processing task that is used for basic traffic cone detection
+ * Using OpenCV 2.4.9
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, libraries compiled for cross compilation 16.10.2017
+ * M.Ozcelikors <mozcelikors@gmail.com>, demonstrator image processing task added, 19.10.2017
+ *
+ *
+ * Usage Instructions:
+ * 1) To cross compile OpenCV, make sure you install OpenCV in your Raspberry Pi
+ * 2) Then find opencv shared objects using the following command
+ * find / -name libopencv*.so
+ * find / -name libopencv*.so*
+ * 3) Copy all of the found .so files to your C:\SysGCC\Raspberry\arm-linux-gnueabihf\sysroot\lib\arm-linux-gnueabihf (on Windows)
+ * 4) Be sure to use following Linker flags: (adjust the path to your system):
+ * -Wl,-verbose,-rpath-link,"C:\SysGCC\Raspberry\arm-linux-gnueabihf\sysroot\lib\arm-linux-gnueabihf"
+ * 5) Be sure to add the libraries you are using to the linker using -l:
+ * such as ... -lopencv_core -lopencv_ml -lopencv_imgproc
+ * 6) Be sure to include the library search paths using -L:
+ * -L"C:\SysGCC\Raspberry\arm-linux-gnueabihf\sysroot\lib\arm-linux-gnueabihf"
+ * 7) Be sure to include OpenCV and raspicam libraries below to `include directories` in gcc using -I flag.
+ *
+ * IMPORTANT!: For this task to run, camera stream or any other processes that use the raspberry pi camera should be deactivated!
+ *
+ */
+
+#include <tasks/image_processing_task.h>
+
+#include <ctime>
+#include <wiringPi.h>
+#include <unistd.h>
+#include <libraries/timing/timing.h>
+#include <api/basic_psys_rover.h>
+#include <interfaces.h>
+#include <pthread.h>
+
+#include <libraries/pthread_monitoring/collect_thread_name.h>
+#include <roverapp.h>
+
+#include <iostream>
+#include <fstream>
+
+/* OpenCV 2.4.9 libraries, Includes (-I) are in /workspace, include paths are specified for "g++"!! */
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/opencv.hpp>
+
+/* raspicam-0.1.3 libraries, Includes (-I) are in /workspace, include paths are specified for "g++"!! */
+#include <raspicam.h>
+#include <raspicam_cv.h>
+
+using namespace cv;
+using namespace std;
+
+void *Image_Processing_Task(void *arg)
+{
+ timing imgproc_task_tmr;
+
+ CollectThreadName("Image_Processing_Task");
+
+ imgproc_task_tmr.setTaskID("ImgPr");
+ imgproc_task_tmr.setDeadline(1);
+ imgproc_task_tmr.setPeriod(1);
+
+ /*
+ char key;
+ char* output_window_name = "Camera Output";
+ char* grayscale_window_name = "Grayscale Image";
+ char* thresholded_window_name = "Thresholded Image";
+ char* contours_window_name = "Contours Image";
+
+ int first_time_cmp = 1;
+
+ int canny_lowthreshold = 20;
+ int canny_highthreshold = 50;
+ int width_bound = 16;//5
+ int height_bound = 16;//5
+ int solidity_bound = 9;
+ int max_val = 200;
+ int float_max_val = 30;
+ int color_max_val = 256;
+
+ //Orange
+ int u_h=0;//0;//0;//0; //0
+ int u_s=177;//148;//102;//134;//120; //197
+ int u_v=178;//149;//220;//212;//120; //189
+ int d_h=203;//5;//256;//256; //9
+ int d_s=256;//256; //256
+ int d_v=256;//256; //256
+
+ int up_aspectRatio = 26;
+ int down_aspectRatio = 15;
+ int aspect_max_val = 50;
+ RNG rng(12345);
+
+ int detections_w[10];
+ int detections_h[10];
+ int detections_x[10];
+ int detections_y[10];
+ double detections_wh[10];
+ int detection_count = 0;
+
+ int send_flag = 0;
+
+ namedWindow(output_window_name, CV_WINDOW_AUTOSIZE);
+ namedWindow(grayscale_window_name, 100);
+ namedWindow(thresholded_window_name, 100);
+ namedWindow(contours_window_name, 100);
+
+ moveWindow(grayscale_window_name, 20,20);
+ moveWindow(thresholded_window_name, 20, 250);
+ moveWindow(contours_window_name, 20, 600);
+
+
+ vector<int> compression_params;
+ compression_params.push_back(CV_IMWRITE_JPEG_QUALITY);
+ compression_params.push_back(100);
+
+ raspicam::RaspiCam_Cv Camera;
+ //Camera.set (CV_CAP_PROP_FORMAT, CV_8UC3);//CV_BGR2HSV); // Original format in Raspicam -> CV_8UC1);
+ if (!Camera.open()){
+ printf("Camera failed to open!\n");
+ abort();
+ }
+
+ Mat frame;
+ Mat frameRGB;
+ Mat imgGrayScale;
+ Mat imgHsv;
+ Mat imgRedThresh;
+ Mat imgWhiteThresh;
+ Mat imgResultingThresh;
+ Mat edges;
+ vector< vector<Point> > contours;
+ vector< Vec4i > hierarchy;
+ vector<Point> approx;
+ vector<Point> tempCont;
+ vector<vector<Point> > tempVecCont;
+ double contPeri;
+ Rect bBox;
+ double aspectRatio;
+
+ double area;
+ double hullArea;
+ double solidity;
+
+ int opening_length;
+ int opening_midpoint;
+
+ double maximum ;
+ double second_maximum ;
+ double third_maximum;
+ int maximum_idx ;
+ int second_maximum_idx ;
+ int third_maximum_idx;
+ double second_last_maximum;
+ double last_maximum;
+
+ int counter = 0;
+ int counter_max = 1;
+ //createTrackbar("canny low Threshold: ", output_window_name, &canny_lowthreshold, max_val, canny);
+ //createTrackbar("canny high Threshold: ", output_window_name, &canny_highthreshold, max_val, canny);
+ //createTrackbar("height: ", output_window_name, &height_bound, float_max_val, canny);
+ //createTrackbar("width: ", output_window_name, &width_bound, float_max_val, canny);
+ //createTrackbar("solidity: ", output_window_name, &solidity_bound, float_max_val, canny);
+ //createTrackbar("Up AspectRatio: ", output_window_name, &up_aspectRatio, aspect_max_val, canny);
+ //createTrackbar("Down AspectRatio: ", output_window_name, &down_aspectRatio, aspect_max_val, canny);
+ //createTrackbar("Up H: ", output_window_name, &u_h, color_max_val, canny);
+ //createTrackbar("Up S: ", output_window_name, &u_s, color_max_val, canny);
+ //createTrackbar("Up v: ", output_window_name, &u_v, color_max_val, canny);
+ //createTrackbar("Down H: ", output_window_name, &d_h, color_max_val, canny);
+ //createTrackbar("Down S: ", output_window_name, &d_s, color_max_val, canny);
+ //createTrackbar("Down v: ", output_window_name, &d_v, color_max_val, canny);
+*/
+
+ while (1)
+ {
+ imgproc_task_tmr.recordStartTime();
+ imgproc_task_tmr.calculatePreviousSlackTime();
+
+ //Task content starts here -----------------------------------------------
+/*
+ //Capture image with Raspicam_CV
+ Camera.grab();
+ Camera.retrieve(frame); //cap >> frame
+
+ //cv::imwrite("raspicam_img.jpg", frame);
+
+ //printf("Starting...\n");
+ //key = waitKey(10);
+ //cout <<(int) char(key) << endl;
+ //if(char(key) == 27){
+ // break;
+ //}
+ // if(char(key) == 10){
+
+ frameRGB.create(frame.size(), frame.type());
+ cvtColor(frame, frameRGB, CV_BGR2RGB);
+
+ imgHsv.create(frameRGB.size(), frameRGB.type());
+ cvtColor(frameRGB, imgHsv, CV_RGB2HSV);//HSV
+
+ //cv::imwrite("raspicam_img.jpg", imgHsv);
+
+ inRange(imgHsv, Scalar(u_h, u_s, u_v), Scalar(d_h, d_s, d_v), imgRedThresh);
+
+ //converting the original image into grayscale
+ imgGrayScale.create(frame.size(), frame.type());
+ cvtColor(frame, imgGrayScale, CV_BGR2GRAY);
+ bitwise_and(imgRedThresh, imgGrayScale, imgGrayScale);
+
+ // Floodfill from point (0, 0)
+ Mat im_floodfill = imgGrayScale.clone();
+ floodFill(im_floodfill, cv::Point(0,0), Scalar(255));
+
+ // Invert floodfilled image
+ Mat im_floodfill_inv;
+ bitwise_not(im_floodfill, im_floodfill_inv);
+
+ // Combine the two images to get the foreground.
+ imgGrayScale = (imgGrayScale | im_floodfill_inv);
+
+ GaussianBlur(imgGrayScale, imgGrayScale, Size(7,7), 1.5, 1.5);
+
+ //imshow(grayscale_window_name, imgGrayScale);
+
+ edges.create(imgGrayScale.size(), imgGrayScale.type());
+ Canny(imgGrayScale, edges, canny_lowthreshold, canny_highthreshold, 3);
+
+ findContours(edges, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0,0));
+ Mat drawing = Mat::zeros(edges.size(), CV_8UC3);
+
+ //For all contours
+ for(int i=0 ; i<contours.size() ; i++){
+ contPeri = arcLength(contours.at(i), true);
+ approxPolyDP(contours.at(i), approx, 0.01 * contPeri, true);
+
+ //if(approx.size()>=7 && approx.size()<=9){
+ bBox = boundingRect(approx);
+ aspectRatio = bBox.width/bBox.height;
+
+ area = contourArea(contours.at(i));
+ convexHull(contours.at(i), tempCont);
+ hullArea = contourArea(tempCont);
+ solidity = area / hullArea;
+
+ // cout << "contour : " << approx.size() << endl;
+ // cout << bBox.width << "::" << bBox.height << "::" << solidity << "::" << aspectRatio <<endl;
+
+ tempVecCont.clear();
+ tempVecCont.push_back(approx);
+
+ if(bBox.width > width_bound && bBox.height > height_bound && solidity > solidity_bound/10 )
+ {
+ //Find detections
+ //printf("%d\t%d\n",bBox.width, bBox.height);
+ detections_x[detection_count] = bBox.x;
+ detections_y[detection_count] = bBox.y;
+ detections_w[detection_count] = bBox.width;
+ detections_h[detection_count] = bBox.height;
+ detections_wh[detection_count] = bBox.width*bBox.height;
+ detection_count = detection_count + 1;
+
+ //Scalar color = Scalar(rng.uniform(0,255), rng.uniform(0,255), rng.uniform(0,255));
+ //drawContours(drawing, tempVecCont, 0, color, 2, 8, hierarchy, 0, Point());
+ //rectangle( drawing, Point( bBox.x, bBox.y), Point( bBox.x+bBox.width, bBox.y+bBox.height), Scalar( 0, 55, 255 ), +4, 4 );
+ }
+ //}
+
+ }
+ //First we shall select the biggest two boxes, having biggest w x h.
+ maximum = detections_wh[0];
+ second_maximum = detections_wh[0];
+ third_maximum = detections_wh[0];
+ maximum_idx = 0;
+ second_maximum_idx = 0;
+ third_maximum_idx = 0;
+ int c;
+ for (c = 0; c < detection_count; c++)
+ {
+ if (detections_wh[c] > maximum)
+ {
+ third_maximum = second_maximum;
+ third_maximum_idx = second_maximum_idx;
+ second_maximum = maximum;
+ second_maximum_idx = maximum_idx;
+ maximum = detections_wh[c];
+ maximum_idx = c;
+ }
+ }
+
+ if(maximum > 10000 || second_maximum > 10000 || third_maximum > 10000) //was 3000
+ {
+ send_flag = 1;
+ //Clear detection width*height array after command is sent
+ for (c = 0; c<detection_count; c++)
+ {
+ detections_wh[c] = 0;
+ }
+ }
+ else
+ {
+ ofstream myfile;
+ myfile.open("../../logs/image_processing/detection.inc");
+ myfile << "undetected";
+ myfile.close();
+ }
+ second_last_maximum = second_maximum;
+ last_maximum = maximum;
+
+
+ if(send_flag == 1){
+
+ printf("CONTOUR DETECTED\n");
+ counter = 0;
+ send_flag = 0;
+ }
+
+
+
+ counter++;
+ */
+
+ //Task content ends here -------------------------------------------------
+
+ imgproc_task_tmr.recordEndTime();
+ imgproc_task_tmr.calculateExecutionTime();
+ imgproc_task_tmr.calculateDeadlineMissPercentage();
+ imgproc_task_tmr.incrementTotalCycles();
+ pthread_mutex_lock(&imgproc_task_ti_l);
+ imgproc_task_ti.deadline = imgproc_task_tmr.getDeadline();
+ imgproc_task_ti.deadline_miss_percentage = imgproc_task_tmr.getDeadlineMissPercentage();
+ imgproc_task_ti.execution_time = imgproc_task_tmr.getExecutionTime();
+ imgproc_task_ti.period = imgproc_task_tmr.getPeriod();
+ imgproc_task_ti.prev_slack_time = imgproc_task_tmr.getPrevSlackTime();
+ imgproc_task_ti.task_id = imgproc_task_tmr.getTaskID();
+ imgproc_task_ti.start_time = imgproc_task_tmr.getStartTime();
+ imgproc_task_ti.end_time = imgproc_task_tmr.getEndTime();
+ pthread_mutex_unlock(&imgproc_task_ti_l);
+ imgproc_task_tmr.sleepToMatchPeriod();
+ }
+
+ // Destroy the screens
+ /*destroyWindow(output_window_name);
+ destroyWindow(thresholded_window_name);
+ destroyWindow(grayscale_window_name);
+ destroyWindow(contours_window_name);*/
+
+ //Close raspicam
+ //cap.release(); --> In webcam
+ /*Camera.release();*/
+
+ /* the function must return something - NULL will do */
+ return NULL;
+}
diff --git a/rover/src/tasks/socket_client_task.cpp b/rover/src/tasks/socket_client_task.cpp
new file mode 100644
index 0000000..7775c3a
--- /dev/null
+++ b/rover/src/tasks/socket_client_task.cpp
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2017 FH Dortmund and others
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ * Socket client for rover-app to rover-web communication
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created 26.10.2017
+ *
+ */
+
+#include <tasks/socket_client_task.h>
+
+#include <wiringPi.h>
+#include <unistd.h>
+#include <ctime>
+#include <libraries/timing/timing.h>
+#include <api/basic_psys_rover.h>
+#include <interfaces.h>
+#include <pthread.h>
+
+#include <libraries/pthread_monitoring/collect_thread_name.h>
+#include <roverapp.h>
+
+#include <unistd.h>
+#include <string.h>
+
+/* Signal header */
+#include <signal.h>
+
+/* Socket defs header */
+#include <socket_settings.h>
+
+/* json-cpp library */
+#include <json/json.h>
+
+#include <math.h>
+
+/* Global definitions */
+int roverapp_send_sockfd;
+
+/* If returns nonzero -> socket has a non zero error status */
+int checkClientSocketStatus (int socket_fd)
+{
+ int error = 0;
+ socklen_t len = sizeof (error);
+ int retval = getsockopt (socket_fd, SOL_SOCKET, SO_ERROR, &error, &len);
+ return error;
+}
+
+Json::Value constructJSONData (int data_type)
+{
+ Json::Value data;
+ switch (data_type)
+ {
+ case SENSOR_DATA:
+ data["rover_dtype"] = "sensor";
+ data["data"]["infrared0"] = ceil(infrared_shared[0]);
+ data["data"]["infrared1"] = ceil(infrared_shared[1]);
+ data["data"]["infrared2"] = ceil(infrared_shared[2]);
+ data["data"]["infrared3"] = ceil (infrared_shared[3]);
+ data["data"]["front"] = ceil (distance_sr04_front_shared);
+ data["data"]["rear"] = ceil (distance_sr04_back_shared);
+ data["data"]["temperature"] = ceil (temperature_shared);
+ data["data"]["humidity"] = ceil (humidity_shared);
+ data["data"]["bearing"] = ceil (bearing_shared);
+ break;
+ case UTIL_DATA:
+ data["rover_dtype"] = "util";
+ data["data"]["core0"] = cpu_util_shared[0];
+ data["data"]["core1"] = cpu_util_shared[1];
+ data["data"]["core2"] = cpu_util_shared[2];
+ data["data"]["core3"] = cpu_util_shared[3];
+ break;
+ default:
+ data["rover_dtype"] = "none";
+ break;
+ }
+ return data;
+}
+
+void Socket_Client_Task_Terminator (int dummy)
+{
+ close(roverapp_send_sockfd);
+ running_flag = 0;
+}
+
+void *Socket_Client_Task (void * arg)
+{
+ timing socket_client_task_tmr;
+
+ CollectThreadName("Socket_Client_Task");
+
+ socket_client_task_tmr.setTaskID("Socket_Client_Task");
+ socket_client_task_tmr.setDeadline(0.8);
+ socket_client_task_tmr.setPeriod(0.8);
+
+ /* Add termination signal handler to properly close socket */
+ signal(SIGINT, Socket_Client_Task_Terminator);
+ signal(SIGTERM, Socket_Client_Task_Terminator);
+ signal(SIGKILL, Socket_Client_Task_Terminator);
+
+ /* Socket related initializations */
+ int n;
+ struct sockaddr_in serv_addr;
+ struct hostent *server;
+ char client_buffer[JSON_DATA_BUFSIZE];
+ char * eof_str = "\r\n";
+ int connected_flag = 0;
+
+ /* Send these data each iteration*/
+ int job_count = 2;
+ int job_list[job_count] = {SENSOR_DATA, UTIL_DATA};
+ int job_idx = 0;
+
+ /* Create JSON object to hold sensor data */
+ Json::FastWriter string_writer;
+
+ while (running_flag)
+ {
+ socket_client_task_tmr.recordStartTime();
+ socket_client_task_tmr.calculatePreviousSlackTime();
+
+ //Task content starts here -----------------------------------------------
+ /* For Connecting / Reconnecting */
+ if (checkClientSocketStatus(roverapp_send_sockfd)!=0)
+ {
+ connected_flag = 0;
+ }
+
+ if (connected_flag == 0)
+ {
+ /* Try to connect or reconnect */
+ /* Create socket, and connect to the socket */
+ close(roverapp_send_sockfd);
+ roverapp_send_sockfd = socket(AF_INET, SOCK_STREAM, 0);
+ if (roverapp_send_sockfd < 0)
+ perror("ERROR opening socket");
+ server = gethostbyname(CONNECTION_HOSTNAME);
+ if (server == NULL) {
+ fprintf(stderr,"ERROR, no such host\n");
+ exit(0);
+ }
+ bzero((char *) &serv_addr, sizeof(serv_addr));
+ serv_addr.sin_family = AF_INET;
+ bcopy((char *)server->h_addr,
+ (char *)&serv_addr.sin_addr.s_addr,
+ server->h_length);
+ serv_addr.sin_port = htons(ROVERAPP_SEND_PORT);
+
+ /* Try to connect */
+ if (connect(roverapp_send_sockfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) == 0)
+ {
+ connected_flag = 1;
+ job_idx = 0;
+ }
+ }
+ else
+ {
+ /* Connected, start writing */
+ /* Construct the JSON data and Write to string */
+ std::string temp = string_writer.write(constructJSONData(job_list[job_idx++]));
+ if (job_idx == job_count)
+ job_idx = 0;
+
+ /* Clear buffer and Write to buffer from string */
+ bzero(client_buffer, JSON_DATA_BUFSIZE);
+ if (strncpy(client_buffer, temp.c_str(), sizeof(client_buffer)) == NULL)
+ {
+ perror("strcpy temp");
+ }
+ client_buffer[sizeof(client_buffer)-1] = 0;
+
+ /* Attach end of file character to string*/
+ if (strcat(client_buffer, eof_str) == NULL)
+ {
+ perror("strcat EOF");
+ }
+
+ /* Write to socket */
+ if (write(roverapp_send_sockfd, client_buffer, strlen(client_buffer)) == -1)
+ {
+ perror("write");
+ /* Very important approach to have re-usable socket client */
+ /* Checking connection status by using exit status of 'write' */
+ printf ("Disconnected from server : socket_client.cpp");
+ connected_flag = 0;
+ }
+ }
+
+ //Task content ends here -------------------------------------------------
+ socket_client_task_tmr.recordEndTime();
+ socket_client_task_tmr.calculateExecutionTime();
+ socket_client_task_tmr.calculateDeadlineMissPercentage();
+ socket_client_task_tmr.incrementTotalCycles();
+ pthread_mutex_lock(&socket_client_task_ti_l);
+ socket_client_task_ti.deadline = socket_client_task_tmr.getDeadline();
+ socket_client_task_ti.deadline_miss_percentage = socket_client_task_tmr.getDeadlineMissPercentage();
+ socket_client_task_ti.execution_time = socket_client_task_tmr.getExecutionTime();
+ socket_client_task_ti.period = socket_client_task_tmr.getPeriod();
+ socket_client_task_ti.prev_slack_time = socket_client_task_tmr.getPrevSlackTime();
+ socket_client_task_ti.task_id = socket_client_task_tmr.getTaskID();
+ socket_client_task_ti.start_time = socket_client_task_tmr.getStartTime();
+ socket_client_task_ti.end_time = socket_client_task_tmr.getEndTime();
+ pthread_mutex_unlock(&socket_client_task_ti_l);
+ socket_client_task_tmr.sleepToMatchPeriod();
+ }
+
+ /* the function must return something - NULL will do */
+ return NULL;
+}
+
+
+
diff --git a/rover/src/tasks/socket_server_task.cpp b/rover/src/tasks/socket_server_task.cpp
new file mode 100644
index 0000000..33ddd77
--- /dev/null
+++ b/rover/src/tasks/socket_server_task.cpp
@@ -0,0 +1,234 @@
+/*
+ * Copyright (c) 2017 FH Dortmund and others
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ * Socket server for rover-web to rover-app communication
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created 26.10.2017
+ *
+ */
+
+#include <tasks/socket_server_task.h>
+
+#include <string.h>
+#include <ctime>
+#include <wiringPi.h>
+#include <unistd.h>
+#include <libraries/timing/timing.h>
+#include <api/basic_psys_rover.h>
+#include <interfaces.h>
+#include <pthread.h>
+#include <softPwm.h>
+
+#include <libraries/pthread_monitoring/collect_thread_name.h>
+#include <roverapp.h>
+
+/* Signal header */
+#include <signal.h>
+
+/* Socket defs header */
+#include <socket_settings.h>
+
+/* json-cpp library */
+#include <json/json.h>
+
+/* Global definitions */
+int roverapp_listen_sockfd;
+int newroverapp_listen_sockfd;
+
+/* If returns nonzero -> socket has a non zero error status */
+int checkServerSocketStatus (int socket_fd)
+{
+ //TODO: Fix this function!
+ int error = 0;
+ socklen_t len = sizeof (error);
+ int retval = getsockopt (socket_fd, SOL_SOCKET, SO_ERROR, &error, &len);
+ return error;
+}
+
+void parseJSONData (char *server_buffer)
+{
+ Json::Value root;
+ Json::Reader reader;
+ bool parsingSuccessful = reader.parse( server_buffer, root );
+ if ( !parsingSuccessful )
+ {
+ std::cout << "Failed to parse" << reader.getFormattedErrorMessages();
+ }
+
+ if (root["rover_dtype"].asString() == "control")
+ {
+ // Update shared variable
+ pthread_mutex_lock(&keycommand_lock);
+ //Take only the first char - since interface uses 1 char only
+ keycommand_shared = root["data"]["command"].asString()[0];
+ //printf("RECV=%c\n",keycommand_shared);
+ pthread_mutex_unlock(&keycommand_lock);
+ }
+ else if (root["rover_dtype"].asString() == "speed")
+ {
+ // Update shared variable
+ pthread_mutex_lock(&speed_lock);
+ speed_shared = root["data"]["speed"].asInt();
+ //printf("RECV=%d\n",speed_shared);
+ pthread_mutex_unlock(&speed_lock);
+ }
+ else
+ {
+ std::cout << "Unable to parse in socket_server_task.cpp" << std::endl;
+ }
+
+}
+
+void Socket_Server_Task_Terminator (int dummy)
+{
+ close(roverapp_listen_sockfd);
+ close(newroverapp_listen_sockfd);
+ running_flag = 0;
+}
+
+void *Socket_Server_Task(void * arg)
+{
+ timing socket_server_task_tmr;
+
+ CollectThreadName("Socket_Server_Task");
+
+ socket_server_task_tmr.setTaskID("Socket_Server_Task");
+ socket_server_task_tmr.setDeadline(0.05);
+ socket_server_task_tmr.setPeriod(0.05);
+
+ /* Add termination signal handler to properly close socket */
+ signal(SIGINT, Socket_Server_Task_Terminator);
+ signal(SIGTERM, Socket_Server_Task_Terminator);
+ signal(SIGKILL, Socket_Server_Task_Terminator);
+
+ socklen_t clilen;
+ char server_buffer[JSON_DATA_BUFSIZE];
+ struct sockaddr_in serv_addr, cli_addr;
+ int n;
+ int true_ = 1;
+ int client_connected_flag = 0;
+
+ /* First call to socket() function */
+ roverapp_listen_sockfd = socket(AF_INET, SOCK_STREAM, 0);
+
+ if (roverapp_listen_sockfd < 0) {
+ perror("ERROR opening socket");
+ exit(1);
+ }
+
+ /* Handle re-binding problems */
+ setsockopt(roverapp_listen_sockfd, SOL_SOCKET,SO_REUSEADDR,&true_,sizeof(int));
+
+ /* Initialize socket structure */
+ bzero((char *) &serv_addr, sizeof(serv_addr));
+
+ serv_addr.sin_family = AF_INET;
+ serv_addr.sin_addr.s_addr = inet_addr(BINDING_HOSTNAME);
+ serv_addr.sin_port = htons(ROVERAPP_LISTEN_PORT);
+ memset(serv_addr.sin_zero, '\0', sizeof serv_addr.sin_zero);
+
+ /* Now bind the host address using bind() call.*/
+ if (bind(roverapp_listen_sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
+ perror("ERROR on binding");
+ exit(1);
+ }
+
+ /* Now start listening for the clients, here process will
+ * go in sleep mode and will wait for the incoming connection
+ */
+ listen(roverapp_listen_sockfd,5); // Max 5 connections queued
+ clilen = sizeof(cli_addr);
+
+ while(running_flag)
+ {
+ socket_server_task_tmr.recordStartTime();
+ socket_server_task_tmr.calculatePreviousSlackTime();
+
+ //Task content starts here -----------------------------------------------
+ /* Handle client acception or re-acception */
+ /* Checking if client socket is available */
+ if (checkServerSocketStatus(newroverapp_listen_sockfd)!=0)
+ {
+ client_connected_flag = 0;
+ }
+
+ if (client_connected_flag == 0)
+ {
+ /* If no client is connected currently */
+ /* Accept actual connection from the client */
+ newroverapp_listen_sockfd = accept(roverapp_listen_sockfd, (struct sockaddr *)&cli_addr, &clilen);
+
+ if (newroverapp_listen_sockfd < 0) {
+ perror("ERROR on accept -> newroverapp_listen_sockfd");
+ exit(1);
+ }
+ else
+ {
+ printf ("Client connected to newroverapp_listen_sockfd \n");
+ client_connected_flag = 1;
+ }
+ }
+ else
+ {
+ /* If a client is connected */
+ /* If connection is established then start communicating */
+ size_t buf_idx = 0;
+
+ /* Read one char at a time until the EOF_STR "\r\n" is reached */
+ while (buf_idx < JSON_DATA_BUFSIZE)
+ {
+ if ( 1 == read(newroverapp_listen_sockfd, &server_buffer[buf_idx], 1))
+ {
+ if (buf_idx > 0 && LINE_FEED == server_buffer[buf_idx] && CARRIAGE_RETURN == server_buffer[buf_idx - 1])
+ {
+ break;
+ }
+ buf_idx++;
+ }
+ else
+ {
+ /* Very important approach to have re-usable socket server */
+ /* Client connection status is determined by exit status of read */
+ printf ("Client disconnected!");
+ client_connected_flag = 0;
+ break;
+ }
+
+ }
+ // Received buffer
+ //printf ("buffer: %s\n",server_buffer);
+
+ /* Remove \r\n from at the end of the string */
+ server_buffer[buf_idx] = '\0';
+ server_buffer[buf_idx-1] = '\0';
+
+ /* Parse the JSON data, and update global variables */
+ parseJSONData(server_buffer);
+ }
+
+ //Task content ends here -------------------------------------------------
+
+ socket_server_task_tmr.recordEndTime();
+ socket_server_task_tmr.calculateExecutionTime();
+ socket_server_task_tmr.calculateDeadlineMissPercentage();
+ socket_server_task_tmr.incrementTotalCycles();
+ pthread_mutex_lock(&socket_server_task_ti_l);
+ socket_server_task_ti.deadline = socket_server_task_tmr.getDeadline();
+ socket_server_task_ti.deadline_miss_percentage = socket_server_task_tmr.getDeadlineMissPercentage();
+ socket_server_task_ti.execution_time = socket_server_task_tmr.getExecutionTime();
+ socket_server_task_ti.period = socket_server_task_tmr.getPeriod();
+ socket_server_task_ti.prev_slack_time = socket_server_task_tmr.getPrevSlackTime();
+ socket_server_task_ti.task_id = socket_server_task_tmr.getTaskID();
+ socket_server_task_ti.start_time = socket_server_task_tmr.getStartTime();
+ socket_server_task_ti.end_time = socket_server_task_tmr.getEndTime();
+ pthread_mutex_unlock(&socket_server_task_ti_l);
+ socket_server_task_tmr.sleepToMatchPeriod();
+
+ }
+}
