Fixed bug with incorrect detection, some cleanup

Author: yurii-litvinov

trikControl/config.xml

@@ -191,6 +191,6 @@ Use it if JCx ports are configured to work as PWM signal generators.
 	<i2c path="/dev/i2c-2" deviceId="0x48" />
 
 	<!-- Settings for virtual camera line sensor provided by rover-cv application. -->
-	<cameraLineDetector roverCv="/home/root/video-module/rover-cv" inputFile="/tmp/dsp-detector.in.fifo" outputFile="/tmp/dsp-detector.out.fifo" />
+	<cameraLineDetector roverCv="/home/root/video-module/rover-cv" inputFile="/tmp/dsp-detector.in.fifo" outputFile="/tmp/dsp-detector.out.fifo" toleranceFactor="5" />
 
 </config>

trikControl/include/trikControl/cameraLineDetectorSensor.h

@@ -33,7 +33,15 @@ class TRIKCONTROL_EXPORT CameraLineDetectorSensor : public Sensor
 	Q_OBJECT
 
 public:
-	CameraLineDetectorSensor(QString const &roverCvBinary, QString const &inputFile, QString const &outputFile);
+	/// Constructor.
+	/// @param roverCvBinary - binary file of rover-cv program which is used to work with video
+	/// @param inputFile - rover-cv input fifo. Note that we will write data here, not read it.
+	/// @param outputFile - rover-cv output fifo. Note that we will read sensor data from here.
+	/// @param toleranceFactor - a value on which hueTolerance, saturationTolerance and valueTolerance is multiplied
+	///        after "detect" command. Higher values allow to count more points on an image as tracked object.
+	CameraLineDetectorSensor(QString const &roverCvBinary, QString const &inputFile
+			, QString const &outputFile, double toleranceFactor);
+
 	~CameraLineDetectorSensor();
 
 public slots:
@@ -47,7 +55,10 @@ public slots:
 	int read();
 
 private:
+	/// Worker object that handles sensor in separate thread.
 	QScopedPointer<CameraLineDetectorSensorWorker> mCameraLineDetectorSensorWorker;
+
+	/// Worker thread.
 	QThread mWorkerThread;
 };
 

trikControl/src/brick.cpp

@@ -127,6 +127,7 @@ Brick::Brick(QThread &guiThread, QString const &configFilePath)
 	mCameraLineDetectorSensor = new CameraLineDetectorSensor(mConfigurer->roverCvBinary()
 			, mConfigurer->roverCvInputFile()
 			, mConfigurer->roverCvOutputFile()
+			, mConfigurer->roverCvToleranceFactor()
 			);
 }
 

trikControl/src/cameraLineDetectorSensorWorker.cpp

@@ -25,16 +25,18 @@
 
 using namespace trikControl;
 
-CameraLineDetectorSensorWorker::CameraLineDetectorSensorWorker(
-		QString const &roverCvBinary
+CameraLineDetectorSensorWorker::CameraLineDetectorSensorWorker(QString const &roverCvBinary
 		, QString const &inputFile
-		, QString const &outputFile)
+		, QString const &outputFile
+		, double toleranceFactor)
 	: mReading(0)
 	, mRoverCvBinary(roverCvBinary)
-	, mOutputFileDescriptor(0)
+	, mOutputFileDescriptor(-1)
+	, mRoverCvProcess(this)
 	, mInputFile(inputFile)
 	, mOutputFile(outputFile)
 	, mReady(false)
+	, mToleranceFactor(toleranceFactor)
 {
 	qRegisterMetaType<QProcess::ProcessError>("QProcess::ProcessError");
 
@@ -53,13 +55,9 @@ CameraLineDetectorSensorWorker::~CameraLineDetectorSensorWorker()
 	deinitialize();
 }
 
-void CameraLineDetectorSensorWorker::moveChildrenToCorrectThread()
-{
-	mRoverCvProcess.moveToThread(this->thread());
-}
-
 void CameraLineDetectorSensorWorker::init()
 {
+	// Since rover-cv can die silently, we need to check not only mReady flag, but also input and output fifos.
 	if (!mReady || !mInputFile.exists() || !mOutputFile.exists()) {
 		initDetector();
 	}
@@ -162,15 +160,16 @@ void CameraLineDetectorSensorWorker::readFile()
 			int const value = parsedLine[5].toInt();
 			int const valueTolerance = parsedLine[6].toInt();
 
-			// These values are not needed in current implementation, but are left here for reference too.
-			Q_UNUSED(hue)
-			Q_UNUSED(hueTolerance)
-			Q_UNUSED(saturation)
-			Q_UNUSED(saturationTolerance)
-			Q_UNUSED(value)
-			Q_UNUSED(valueTolerance)
+			mInputStream
+					<< QString("hsv %0 %1 %2 %3 %4 %5 %6")
+							.arg(hue)
+							.arg(hueTolerance * mToleranceFactor)
+							.arg(saturation)
+							.arg(saturationTolerance * mToleranceFactor)
+							.arg(value)
+							.arg(valueTolerance * mToleranceFactor)
+					<< "\n";
 
-			mInputStream << QString(line).remove(":") << "\n";
 			mInputStream.flush();
 		}
 	}
@@ -214,7 +213,7 @@ void CameraLineDetectorSensorWorker::openFifos()
 {
 	qDebug() << "opening" << mOutputFile.fileName();
 
-	if (!mOutputFileDescriptor) {
+	if (mOutputFileDescriptor == -1) {
 		mOutputFileDescriptor = open(mOutputFile.fileName().toStdString().c_str(), O_SYNC | O_NONBLOCK, O_RDONLY);
 	}
 
@@ -223,12 +222,10 @@ void CameraLineDetectorSensorWorker::openFifos()
 		return;
 	}
 
-	qDebug() << mOutputFileDescriptor;
-
 	mSocketNotifier.reset(new QSocketNotifier(mOutputFileDescriptor, QSocketNotifier::Read));
 
 	connect(mSocketNotifier.data(), SIGNAL(activated(int)), this, SLOT(readFile()));
-	mSocketNotifier->setEnabled(false);
+	mSocketNotifier->setEnabled(true);
 
 	qDebug() << "opening" << mInputFile.fileName();
 
@@ -260,8 +257,10 @@ void CameraLineDetectorSensorWorker::tryToExecute()
 
 void CameraLineDetectorSensorWorker::deinitialize()
 {
-	disconnect(mSocketNotifier.data(), SIGNAL(activated(int)), this, SLOT(readFile()));
-	mSocketNotifier->setEnabled(false);
+	if (mSocketNotifier) {
+		disconnect(mSocketNotifier.data(), SIGNAL(activated(int)), this, SLOT(readFile()));
+		mSocketNotifier->setEnabled(false);
+	}
 	if (::close(mOutputFileDescriptor) != 0) {
 		qDebug() << mOutputFile.fileName() << ": fifo close failed: " << errno;
 	}

trikControl/src/cameraLineDetectorSensorWorker.h

@@ -23,19 +23,25 @@
 #include <QtCore/QTextStream>
 #include <QtCore/QList>
 
-#include "declSpec.h"
-#include "sensor.h"
-
 namespace trikControl {
 
-class TRIKCONTROL_EXPORT CameraLineDetectorSensorWorker : public Sensor
+/// Worker object that processes rover-cv fifo and updates stored reading.
+class CameraLineDetectorSensorWorker : public QObject
 {
 	Q_OBJECT
 
 public:
-	CameraLineDetectorSensorWorker(QString const &roverCvBinary, QString const &inputFile, QString const &outputFile);
+	/// Constructor.
+	/// @param roverCvBinary - binary file of rover-cv program which is used to work with video
+	/// @param inputFile - rover-cv input fifo. Note that we will write data here, not read it.
+	/// @param outputFile - rover-cv output fifo. Note that we will read sensor data from here.
+	/// @param toleranceFactor - a value on which hueTolerance, saturationTolerance and valueTolerance is multiplied
+	///        after "detect" command. Higher values allow to count more points on an image as tracked object.
+	CameraLineDetectorSensorWorker(QString const &roverCvBinary, QString const &inputFile
+				, QString const &outputFile, double toleranceFactor);
+
+	/// Destructor.
 	~CameraLineDetectorSensorWorker();
-	void moveChildrenToCorrectThread();
 
 public slots:
 	/// Initializes a camera and begins showing image from it on display.
@@ -49,30 +55,68 @@ public slots:
 	int read();
 
 private slots:
+	/// Called when rover-cv reports error. Note that it can still die silently.
 	void onRoverCvError(QProcess::ProcessError error);
+
+	/// Called when new data is available on stdout of rover-cv. Note that process stdout is buffered, so this method
+	/// can be called much later than expected.
 	void onRoverCvReadyReadStandardOutput();
+
+	/// Called when new data is available on stderr of rover-cv.
 	void onRoverCvReadyReadStandardError();
 
 	/// Updates current reading when new value is ready.
 	void readFile();
 
 private:
+	/// Starts rover-cv if needed and opens its fifos.
 	void initDetector();
+
+	/// Starts rover-cv process.
 	void startRoverCv();
+
+	/// Opens input and output fifos for rover-cv.
 	void openFifos();
+
+	/// Puts queued commands to rover-cv input fifo and clears queue if rover-cv is ready, otherwise does nothing.
+	/// Life without continuations is pain.
 	void tryToExecute();
+
+	/// Closes fifos. Does not kill rover-cv.
 	void deinitialize();
 
+	/// Listener for output fifo.
 	QScopedPointer<QSocketNotifier> mSocketNotifier;
+
+	/// Current stored reading of a sensor.
 	int mReading;
+
+	/// File name (with path) of rover-cv binary.
 	QString mRoverCvBinary;
+
+	/// File descriptor for output fifo.
 	int mOutputFileDescriptor;
+
+	/// rover-cv process.
 	QProcess mRoverCvProcess;
+
+	/// Input fifo.
 	QFile mInputFile;
+
+	/// Output fifo.
 	QFile mOutputFile;
+
+	/// File stream for command fifo. Despite its name it is used to output commands. It is input for rover-cv.
 	QTextStream mInputStream;
+
+	/// Flag that sensor is ready and waiting for commands.
 	bool mReady;
+
+	/// A queue of commands to be passed to input fifo when it is ready.
 	QList<QString> mCommandQueue;
+
+	/// A value on which hueTolerance, saturationTolerance and valueTolerance is multiplied after "detect" command.
+	double mToleranceFactor;
 };
 
 }

trikControl/src/configurer.cpp

@@ -299,6 +299,11 @@ QString Configurer::roverCvOutputFile() const
 	return mRoverCvOutputFile;
 }
 
+double Configurer::roverCvToleranceFactor() const
+{
+	return mRoverCvToleranceFactor;
+}
+
 void Configurer::loadInit(QDomElement const &root)
 {
 	if (root.elementsByTagName("initScript").isEmpty()) {
@@ -626,4 +631,5 @@ void Configurer::loadCameraLineDetector(QDomElement const &root)
 	mRoverCvBinary = cameraLineDetector.attribute("roverCv");
 	mRoverCvInputFile = cameraLineDetector.attribute("inputFile");
 	mRoverCvOutputFile = cameraLineDetector.attribute("outputFile");
+	mRoverCvToleranceFactor = cameraLineDetector.attribute("toleranceFactor").toDouble();
 }

trikControl/src/configurer.h

@@ -1,4 +1,4 @@
-/* Copyright 2013 Yurii Litvinov
+/* Copyright 2014 CyberTech Labs Ltd.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -132,6 +132,8 @@ class Configurer {
 
 	QString roverCvOutputFile() const;
 
+	double roverCvToleranceFactor() const;
+
 private:
 	struct ServoMotorType {
 		int min;
@@ -233,6 +235,7 @@ class Configurer {
 	QString mRoverCvBinary;
 	QString mRoverCvInputFile;
 	QString mRoverCvOutputFile;
+	double mRoverCvToleranceFactor;
 };
 
 }

trikControl/src/linux/cameraLineDetectorSensor.cpp

@@ -20,14 +20,15 @@
 
 using namespace trikControl;
 
-CameraLineDetectorSensor::CameraLineDetectorSensor(
-		QString const &roverCvBinary
+CameraLineDetectorSensor::CameraLineDetectorSensor(QString const &roverCvBinary
 		, QString const &inputFile
-		, QString const &outputFile)
-	: mCameraLineDetectorSensorWorker(new CameraLineDetectorSensorWorker(roverCvBinary, inputFile, outputFile))
+		, QString const &outputFile
+		, double toleranceFactor)
+	: mCameraLineDetectorSensorWorker(
+			new CameraLineDetectorSensorWorker(roverCvBinary, inputFile, outputFile, toleranceFactor)
+	)
 {
 	mCameraLineDetectorSensorWorker->moveToThread(&mWorkerThread);
-	mCameraLineDetectorSensorWorker->moveChildrenToCorrectThread();
 	mWorkerThread.start();
 }
 
@@ -49,5 +50,6 @@ void CameraLineDetectorSensor::detect()
 
 int CameraLineDetectorSensor::read()
 {
+	// Read is called synchronously and only takes prepared value from sensor.
 	return mCameraLineDetectorSensorWorker->read();
 }