Einbindung der Laserklasse in das bestehende Programm

LaserMeasurement.cpp

@@ -28,29 +28,26 @@ using namespace THOMAS;
 
 LaserMeasurement::LaserMeasurement()
 {
-	_videoCapture = cv::VideoCapture(CAMERA);
 
-	// Kamera Größenparameter setzten
-	_videoCapture.set(CV_CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH);
-	_videoCapture.set(CV_CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT);
-	_videoCapture.set(CV_CAP_PROP_FPS, CAMERA_MAX_FPS);
+}
 
-	cv::Mat frame;
+int LaserMeasurement::GetDistanceFromImage(cv::Mat frame)
+{
+	// Laserposition abrufen
+	float laserPosition = GetLaserPosition(frame, BAR_START, BAR_START + BAR_HEIGHT);
 
-	while(true)
+	// Wert auf Gültigkeit überprüfen
+	if(laserPosition < 0)
 	{
-		_videoCapture >> frame;
-		GetDistanceFromImage(frame);
+		// Spar dir die Berechnung, kommt sowieso Müll raus
+		return 0;
 	}
-}
 
-int LaserMeasurement::GetDistanceFromImage(cv::Mat frame)
-{
-	// Laserposition abrufen und auf Mittelpunkt beziehen
-	float laserPosition = CAMERA_WIDTH / 2 - GetLaserPosition(frame, BAR_START, BAR_START + BAR_HEIGHT);
+	// LaserPosition merken
+	_lastLaserPosition = laserPosition;
 
 	// Distanz berechnen
-	float distance = (-LASER_DISTANCE / tan(ALPHA)) / (2 * laserPosition / CAMERA_WIDTH * tan(GAMMA / 2) / tan(ALPHA) - 1);
+	float distance = (-LASER_DISTANCE / tan(ALPHA)) / (2 * (CAMERA_WIDTH / 2 - laserPosition) / CAMERA_WIDTH * tan(GAMMA / 2) / tan(ALPHA) - 1);
 
 	// Distanz zurückgeben
 	return distance;
@@ -125,6 +122,12 @@ int LaserMeasurement::GetLaserPosition(cv::Mat frame, int startY, int endY)
 	return -1;
 }
 
+int LaserMeasurement::GetLastLaserPosition()
+{
+	// Letze Laserposition zurückgeben
+	return _lastLaserPosition;
+}
+
 int LaserMeasurement::GetBrightnessAvg(std::vector<int> bar, int avgStart, int avgStop)
 {
 	// Division durch 0 verhindern

LaserMeasurement.h

@@ -19,20 +19,15 @@ Diese Klasse misst die Entfernung zum Laserpunkt
 
 /* KONSTANTEN */
 
-// Verwendete Kamera
-#define CAMERA 1
-
 // Kamera Einstellungen
 #define CAMERA_WIDTH 1280
-#define CAMERA_HEIGHT 720
-#define CAMERA_MAX_FPS 30
-
-// Höhe des Erkennungsbalkens
-#define BAR_HEIGHT 60
 
 // Startpunkt (Y) des Erkennungsbalkens
 #define BAR_START 330
 
+// Höhe des Erkennungsbalkens
+#define BAR_HEIGHT 60
+
 // Mindest- und Maximalgröße des Laserpunktes
 #define LASER_MIN_WIDTH 1
 #define LASER_MAX_WIDTH 80
@@ -69,20 +64,24 @@ namespace THOMAS
 	class LaserMeasurement
 	{
 	private:
-		// VideoCapture Device
-		cv::VideoCapture _videoCapture;
-
 		// Gibt die Laserpositon zurück
 		int GetLaserPosition(cv::Mat frame, int startY, int endY);
 
 		// Ermittelt die Helligkeit für einen bestimmten Abschnitt
 		int GetBrightnessAvg(std::vector<int> bar, int avgStart, int avgStop);
 
+		// Die letzte Laserposition
+		int _lastLaserPosition = -1;
+
 	public:
 		// Konstruktor
 		LaserMeasurement();
 
 		// Ermittelt die Entfernung in einem Image
 		int GetDistanceFromImage(cv::Mat frame);
+
+		// Gibt die letzte Laserposition zurück
+		int GetLastLaserPosition();
+
 	};
 }

TelemetryReceiver.cpp

@@ -17,6 +17,9 @@ using namespace THOMAS;
 // Enthält die Kommunikation für den Arduino
 #include "ArduinoProtocol.h"
 
+// Enthält die Funktionen zur Lasergestützten Abstandsmessung
+#include "LaserMeasurement.h"
+
 // OPENCV-Klassen
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
@@ -45,10 +48,10 @@ TelemetryReceiver::TelemetryReceiver()
 
 }
 
-void TelemetryReceiver::Run(ArduinoProtocol *arduinoProtocol, int videoDeviceID)
+void TelemetryReceiver::Run(ArduinoProtocol *arduinoProtocol, LaserMeasurement *laserMeasurement)
 {
 	// Kamera initialisieren
-	_videoCapture = cv::VideoCapture(videoDeviceID);
+	_videoCapture = cv::VideoCapture(CAMERA);
 
 	// Erfolgreich geöffnet
 	if(!_videoCapture.isOpened())
@@ -68,6 +71,9 @@ void TelemetryReceiver::Run(ArduinoProtocol *arduinoProtocol, int videoDeviceID)
 	// ArduinoProtocol speichern
 	_arduino = arduinoProtocol;
 
+	// LaserMeasurement speichern
+	_laser = laserMeasurement;
+
 	// TCP Server initialisieren
 	_server = new TCPServer(4223, ComputeTCPServerDataWrapper, OnClientStatusChangeWrapper, static_cast<void *>(this));
 	_server->BeginListen();
@@ -108,6 +114,26 @@ void TelemetryReceiver::CaptureFrameThread()
 		// Farben in 8Bit konvertieren
 		cvtColor(frame, frame, CV_8U);
 
+		// Letze Laserposition abrufen
+		int lastLaserPosition = _laser->GetLastLaserPosition();
+
+		// Laserposition auf Gültigkeit überprüfen
+		if(lastLaserPosition >= 0)
+		{
+			// Startpunkt bestimmen
+			cv::Point startPoint;
+			startPoint.x = lastLaserPosition;
+			startPoint.y = LASER_MARKER_POSITION - 50;
+
+			// Endpunkt bestimmen
+			cv::Point endPoint;
+			endPoint.x = lastLaserPosition;
+			endPoint.y = LASER_MARKER_POSITION + 50;
+
+			// Position einzeichnen
+			cv::line(frame, startPoint, endPoint, cv::Scalar(0, 0, 255), 3);
+		}
+
 		// JPEG Format setzten
 		param[0] = CV_IMWRITE_JPEG_QUALITY;
 
@@ -127,7 +153,7 @@ void TelemetryReceiver::CaptureFrameThread()
 			cv::Mat clientFrame;
 
 			// Bilder auf die gewünschte Größe anpassen
-			cv::resize(frame,clientFrame, cv::Size(CAMERA_WIDTH*((float) client.second.GetFrameSize()/100),CAMERA_HEIGHT*((float) client.second.GetFrameSize()/100)));
+			cv::resize(frame,clientFrame, cv::Size(CAMERA_WIDTH * ((float) client.second.GetFrameSize() / 100), CAMERA_HEIGHT * ((float) client.second.GetFrameSize() / 100)));
 
 			// Kompressionsrate setzten
 			param[1] = client.second.GetFrameQuality();
@@ -152,7 +178,7 @@ void TelemetryReceiver::CaptureFrameThread()
 				if(i == buff.size() - 1)
 				{
 					// Array mit restlichen Bytes erstellen
-					std::vector<uchar> newBuff(buff.begin() + 64000*((int) buff.size() / 64000), buff.end());
+					std::vector<uchar> newBuff(buff.begin() + 64000 * ((int) buff.size() / 64000), buff.end());
 
 					// Bytes senden
 					client.second.Send(newBuff);

TelemetryReceiver.h

@@ -12,6 +12,9 @@
 // Enthält die Kommunikation für den Arduino
 #include "ArduinoProtocol.h"
 
+// Enthält die Funktionen zur Lasergestützten Abstandsmessung
+#include "LaserMeasurement.h"
+
 // OpenCV implementieren
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
@@ -44,11 +47,17 @@
 #define FIELD_SIGNAL 4
 #define FIELD_BANDWIDTH 5
 
+// Kamera-ID
+#define CAMERA 0
+
 // Kamera Einstellungen
 #define CAMERA_WIDTH 1280
 #define CAMERA_HEIGHT 720
 #define CAMERA_MAX_FPS 30
 
+// Position des Laser-Markierungs-Markers
+#define LASER_MARKER_POSITION 640
+
 namespace THOMAS {
 
 	class TelemetryReceiver
@@ -69,6 +78,9 @@ namespace THOMAS {
 		// ArduinoProtocol Klasse
 		ArduinoProtocol *_arduino;
 
+		// LaserMeasurement Klasse
+		LaserMeasurement *_laser;
+
 		// VideoCapture Device
 		cv::VideoCapture _videoCapture;
 
@@ -112,7 +124,7 @@ namespace THOMAS {
 		TelemetryReceiver();
 
 		// Wird beim Start ausgeführt
-		void Run(ArduinoProtocol *arduinoProtocol, int videoDeviceID = 0);
+		void Run(ArduinoProtocol *arduinoProtocol, LaserMeasurement *laserMeasurement);
 
 	};
 }

main.cpp

@@ -38,9 +38,12 @@ int main(int argc, char **argv)
 	MotorControl *motorControl = new MotorControl();
 	motorControl->Run(arduino);
 
+	// Lasermessung instanzieren
+	LaserMeasurement *laser = new LaserMeasurement();
+
 	// TelemetryReceiver starten
 	TelemetryReceiver *teleRecv = new TelemetryReceiver();
-	teleRecv->Run(arduino);
+	teleRecv->Run(arduino, laser);
 
 	// Programm laufen lassen, Prozessor nicht unnötig belasten (alles läuft in separaten Threads)
 	// TODO: Programm-Befehle per Tastatur etc.