summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--client2.cpp280
1 files changed, 162 insertions, 118 deletions
diff --git a/client2.cpp b/client2.cpp
index d72fe29..9411b72 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -10,6 +10,8 @@
#include <string.h>
#include <sys/un.h>
#include <time.h>
+#include <pthread.h>
+#include <semaphore.h>
#include <opencv2/opencv.hpp>
#include "lib.h"
@@ -459,95 +461,33 @@ float deadzone(float val, float dead)
}
-int main(int argc, const char** argv)
-{
- bool no_oculus = (argc>=2 && (strcmp(argv[1],"--no-oculus")==0));
- GLFWwindow* window = initOpenGL();
-
-
- GLuint vaoCanvas, vaoEye, vaoWholescreenQuad;
- glGenVertexArrays(1, &vaoCanvas);
- glGenVertexArrays(1, &vaoEye);
- glGenVertexArrays(1, &vaoWholescreenQuad);
-
-
- GLuint vboCanvas, vboEye, vboWholescreenQuad;
- glGenBuffers(1, &vboCanvas);
- glGenBuffers(1, &vboEye);
- glGenBuffers(1, &vboWholescreenQuad);
-
- glBindBuffer(GL_ARRAY_BUFFER, vboCanvas);
- glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnCanvasVertices), drawOnCanvasVertices, GL_DYNAMIC_DRAW);
-
- glBindBuffer(GL_ARRAY_BUFFER, vboEye);
- glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnEyeVertices), drawOnEyeVertices, GL_STATIC_DRAW);
-
- glBindBuffer(GL_ARRAY_BUFFER, vboWholescreenQuad);
- glBufferData(GL_ARRAY_BUFFER, sizeof(wholescreenVertices), wholescreenVertices, GL_STATIC_DRAW);
+pthread_mutex_t my_mutex;
+sem_t oculus_thread_go;
+ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180);
+ModuloRingbuffer ringbuf_pitch_sensor_slow(40, -180,180);
+float yaw_cam_global, pitch_cam_global, roll_cam_global;
+Mat frame_global;
+void* video_fetcher_thread(void* ptr)
+{
+ #define RINGBUF_SIZE 4
+ ModuloRingbuffer ringbuf_yaw_cam(RINGBUF_SIZE, -180, 180);
+ Ringbuffer ringbuf_pitch_cam(RINGBUF_SIZE);
+ ModuloRingbuffer ringbuf_roll_cam(RINGBUF_SIZE, -180,180);
+ ModuloRingbuffer ringbuf_roll_sensor(RINGBUF_SIZE, -180,180);
+ ModuloRingbuffer ringbuf_yaw_sensor(RINGBUF_SIZE, -180,180);
+ ModuloRingbuffer ringbuf_pitch_sensor(RINGBUF_SIZE, -180,180);
+ #define DELAY_SIZE 6
+ Ringbuffer delay_phi(DELAY_SIZE); // should delay sensor data by ~0.2sec
+ Ringbuffer delay_psi(DELAY_SIZE); // that's the amount the video lags behind
+ Ringbuffer delay_theta(DELAY_SIZE); // the sensor data
- // compile shaders
- GLuint drawOnOculusProgram = newOculusShaderProgram(vaoWholescreenQuad, vboWholescreenQuad);
- GLint uniAberrR = glGetUniformLocation(drawOnOculusProgram, "aberr_r");
- GLint uniAberrB = glGetUniformLocation(drawOnOculusProgram, "aberr_b");
-
- GLuint drawOnCanvasProgram = newCanvasShaderProgram(vaoCanvas, vboCanvas);
- GLint uniCamYaw = glGetUniformLocation(drawOnCanvasProgram, "cam_yaw");
- GLint uniCamPitch = glGetUniformLocation(drawOnCanvasProgram, "cam_pitch");
- GLint uniCamRoll = glGetUniformLocation(drawOnCanvasProgram, "cam_roll");
+ int adjust_phi=10;
- GLuint drawFromCanvasProgram = newEyeShaderProgram(vaoEye, vboEye);
- GLint uniEyeYaw = glGetUniformLocation(drawFromCanvasProgram, "eye_yaw");
- GLint uniEyePitch = glGetUniformLocation(drawFromCanvasProgram, "eye_pitch");
- GLint uniEyeRoll = glGetUniformLocation(drawFromCanvasProgram, "eye_roll");
-
- // texture
- GLuint texVideo;
- glGenTextures(1, &texVideo);
- glBindTexture(GL_TEXTURE_2D, texVideo);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
-
-
-// Framebuffer stuff
- GLuint canvasFB, canvasTex;
- genFramebuffer(canvasFB, canvasTex, CANVAS_WIDTH, CANVAS_HEIGHT, true);
-
- GLuint eyeFB, eyeTex;
- genFramebuffer(eyeFB, eyeTex, EYE_WIDTH, EYE_HEIGHT, false);
-
-
-
-
-
-
-
- glEnable (GL_BLEND);
- glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-
-
-
-
- ohmd_context* my_ohmd_context;
- ohmd_device* my_ohmd_device;
- if (no_oculus || init_ohmd(&my_ohmd_context, &my_ohmd_device))
- {
- printf("no oculus rift!\n");
- my_ohmd_device=0;
- my_ohmd_context=0;
- }
-
-
-
-
-
DroneConnection drone(SOCKETPATH);
navdata_t navdata;
@@ -558,26 +498,12 @@ int main(int argc, const char** argv)
float px_per_deg = diag / 92.;
- float yaw_cam = 100/px_per_deg, pitch_cam = 0;
+ float yaw_cam = 100/px_per_deg;
+ float pitch_cam = 0;
float roll_cam = 0.0;
Mat frame(Size(1280,720), CV_8UC3), frame_(Size(1280,720), CV_8UC3), gray, oldgray;
- #define RINGBUF_SIZE 4
- ModuloRingbuffer ringbuf_yaw_cam(RINGBUF_SIZE, -180, 180);
- Ringbuffer ringbuf_pitch_cam(RINGBUF_SIZE);
- ModuloRingbuffer ringbuf_roll_cam(RINGBUF_SIZE, -180,180);
- ModuloRingbuffer ringbuf_roll_sensor(RINGBUF_SIZE, -180,180);
- ModuloRingbuffer ringbuf_yaw_sensor(RINGBUF_SIZE, -180,180);
- ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180);
- ModuloRingbuffer ringbuf_pitch_sensor_slow(40, -180,180);
- ModuloRingbuffer ringbuf_pitch_sensor(RINGBUF_SIZE, -180,180);
-
-
- #define DELAY_SIZE 6
- Ringbuffer delay_phi(DELAY_SIZE); // should delay sensor data by ~0.2sec
- Ringbuffer delay_psi(DELAY_SIZE); // that's the amount the video lags behind
- Ringbuffer delay_theta(DELAY_SIZE); // the sensor data
for (int i=0; i<400;i++)
@@ -587,18 +513,9 @@ int main(int argc, const char** argv)
cvtColor(frame, oldgray, COLOR_BGR2GRAY);
}
- char key;
- int adjust_phi=10;
- float aberr_r=0.989, aberr_b=1.021;
- int i=-1;
- while (key=' ')
- //while ((key=waitKey(1)) != 'e')
+ bool first_time = true;
+ while(true)
{
- i++;
- //printf("\033[H");
-
- if (i%10==0)
- {
drone.get(frame_, &navdata);
delay_phi.put(navdata.phi);
delay_psi.put(navdata.psi);
@@ -609,6 +526,7 @@ int main(int argc, const char** argv)
navdata.phi = fixup_angle(navdata.phi + adjust_phi);
+ /*
if (key=='q') adjust_phi++;
if (key=='w') adjust_phi--;
@@ -616,6 +534,7 @@ int main(int argc, const char** argv)
if (key=='z') aberr_r-=0.001;
if (key=='s') aberr_b+=0.001;
if (key=='x') aberr_b-=0.001;
+ */
//printf("aberr_r/b = \t%f\t%f\n",aberr_r,aberr_b);
@@ -624,7 +543,6 @@ int main(int argc, const char** argv)
remap(frame_, frame, map1, map2, INTER_LINEAR);
cvtColor(frame, gray, COLOR_BGR2GRAY);
- }
@@ -636,7 +554,7 @@ int main(int argc, const char** argv)
Mat mat = estimateRigidTransform(gray, oldgray, false);
- float angle; int shift_x, shift_y;
+ float angle; float shift_x, shift_y;
if (mat.total() > 0)
{
angle = atan2(mat.at<double>(0,1), mat.at<double>(0,0)) / PI * 180.;
@@ -661,8 +579,6 @@ int main(int argc, const char** argv)
ringbuf_roll_cam.put(roll_cam);
ringbuf_roll_sensor.put(navdata.phi);
ringbuf_yaw_sensor.put(navdata.psi);
- ringbuf_yaw_sensor_slow.put(navdata.psi);
- ringbuf_pitch_sensor_slow.put(navdata.theta);
ringbuf_pitch_sensor.put(navdata.theta);
double yaw_diff = fixup_range( ringbuf_yaw_cam.get() - ringbuf_yaw_sensor.get(), -180, 180);
@@ -686,24 +602,153 @@ int main(int argc, const char** argv)
//yaw_cam = navdata.psi;
//pitch_cam = - navdata.theta * px_per_deg;
//roll_cam = -navdata.phi;
+
+ pthread_mutex_lock(&my_mutex);
+ yaw_cam_global = yaw_cam;
+ pitch_cam_global = pitch_cam;
+ roll_cam_global = roll_cam;
+ frame_global = frame.clone();
+ ringbuf_yaw_sensor_slow.put(navdata.psi);
+ ringbuf_pitch_sensor_slow.put(navdata.theta);
+ pthread_mutex_unlock(&my_mutex);
+ if (first_time)
+ {
+ sem_post(&oculus_thread_go);
+ first_time=false;
+ }
+
+ oldgray = gray.clone();
+ }
+ return NULL;
+}
+
+int main(int argc, const char** argv)
+{
+ bool no_oculus = (argc>=2 && (strcmp(argv[1],"--no-oculus")==0));
+ pthread_mutex_init(&my_mutex, NULL);
+ sem_init(&oculus_thread_go, 0, 0);
+ GLFWwindow* window = initOpenGL();
+ GLuint vaoCanvas, vaoEye, vaoWholescreenQuad;
+ glGenVertexArrays(1, &vaoCanvas);
+ glGenVertexArrays(1, &vaoEye);
+ glGenVertexArrays(1, &vaoWholescreenQuad);
+ GLuint vboCanvas, vboEye, vboWholescreenQuad;
+ glGenBuffers(1, &vboCanvas);
+ glGenBuffers(1, &vboEye);
+ glGenBuffers(1, &vboWholescreenQuad);
+
+ glBindBuffer(GL_ARRAY_BUFFER, vboCanvas);
+ glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnCanvasVertices), drawOnCanvasVertices, GL_DYNAMIC_DRAW);
+
+ glBindBuffer(GL_ARRAY_BUFFER, vboEye);
+ glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnEyeVertices), drawOnEyeVertices, GL_STATIC_DRAW);
+
+ glBindBuffer(GL_ARRAY_BUFFER, vboWholescreenQuad);
+ glBufferData(GL_ARRAY_BUFFER, sizeof(wholescreenVertices), wholescreenVertices, GL_STATIC_DRAW);
+
+
+
+
+
+ // compile shaders
+ GLuint drawOnOculusProgram = newOculusShaderProgram(vaoWholescreenQuad, vboWholescreenQuad);
+ GLint uniAberrR = glGetUniformLocation(drawOnOculusProgram, "aberr_r");
+ GLint uniAberrB = glGetUniformLocation(drawOnOculusProgram, "aberr_b");
+
+ GLuint drawOnCanvasProgram = newCanvasShaderProgram(vaoCanvas, vboCanvas);
+ GLint uniCamYaw = glGetUniformLocation(drawOnCanvasProgram, "cam_yaw");
+ GLint uniCamPitch = glGetUniformLocation(drawOnCanvasProgram, "cam_pitch");
+ GLint uniCamRoll = glGetUniformLocation(drawOnCanvasProgram, "cam_roll");
+
+ GLuint drawFromCanvasProgram = newEyeShaderProgram(vaoEye, vboEye);
+ GLint uniEyeYaw = glGetUniformLocation(drawFromCanvasProgram, "eye_yaw");
+ GLint uniEyePitch = glGetUniformLocation(drawFromCanvasProgram, "eye_pitch");
+ GLint uniEyeRoll = glGetUniformLocation(drawFromCanvasProgram, "eye_roll");
+
+ // texture
+ GLuint texVideo;
+ glGenTextures(1, &texVideo);
+ glBindTexture(GL_TEXTURE_2D, texVideo);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+
+
+
+// Framebuffer stuff
+ GLuint canvasFB, canvasTex;
+ genFramebuffer(canvasFB, canvasTex, CANVAS_WIDTH, CANVAS_HEIGHT, true);
+
+ GLuint eyeFB, eyeTex;
+ genFramebuffer(eyeFB, eyeTex, EYE_WIDTH, EYE_HEIGHT, false);
+
+
+
+
+
+
+
+ glEnable (GL_BLEND);
+ glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+
+
+
+
+ ohmd_context* my_ohmd_context;
+ ohmd_device* my_ohmd_device;
+ if (no_oculus || init_ohmd(&my_ohmd_context, &my_ohmd_device))
+ {
+ printf("no oculus rift!\n");
+ my_ohmd_device=0;
+ my_ohmd_context=0;
+ }
+
+
+ pthread_t video_thread;
+ pthread_create(&video_thread, NULL, video_fetcher_thread, NULL);
+ sem_wait(&oculus_thread_go);
+
+
+
+ char key;
+ float aberr_r=0.989, aberr_b=1.021;
+ int i=-1;
+ while (key=' ')
+ //while ((key=waitKey(1)) != 'e')
+ {
+ i++;
+ //printf("\033[H");
+
+
+
+
+
+
+
+
+
+ pthread_mutex_lock(&my_mutex);
+ Mat frame_gl = frame_global.clone();
+ float yaw_cam = yaw_cam_global;
+ float pitch_cam = pitch_cam_global;
+ float roll_cam = roll_cam_global;
+ pthread_mutex_unlock(&my_mutex);
- if (i%1==0)
- {
glBindTexture(GL_TEXTURE_2D, texVideo);
- Mat frame_gl = frame.clone();
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, frame_gl.size().width, frame_gl.size().height, 0, GL_RGB, GL_UNSIGNED_BYTE, frame_gl.ptr<unsigned char>(0));
- }
if(i!=0) setDrawOnCanvasRange((float)yaw_cam/180.*PI,-(float)pitch_cam/180.*PI,80./180.*PI,45/180.*PI);
@@ -803,7 +848,6 @@ int main(int argc, const char** argv)
- oldgray = gray.clone();
}