Lidar (#1434)

* Add lidar reading

* Add lidar as sensor

* Simulate lidar

* Add lidar emulation for trikbrick

* Fixes after review

* No need to reread sensor

* Polish lidar working

* Add Read Lidar Block

* Add Read Lidar block to palette

* Add generator for Read Lidar block

* Disable Read Lidar Block

* Need to draw image with real display width and hight

* Lupdate

* Fix build

* Make Vera happy

* Update Runtime

* Make Vera happy

plugins/robots/common/kitBase/include/kitBase/robotModel/robotParts/lidarSensor.h

@@ -0,0 +1,39 @@
+/* Copyright 2021 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.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#pragma once
+
+#include "vectorSensor.h"
+#include "kitBase/kitBaseDeclSpec.h"
+
+namespace kitBase {
+namespace robotModel {
+namespace robotParts {
+
+/// Base class for lidar sensors.
+class ROBOTS_KIT_BASE_EXPORT LidarSensor : public VectorSensor
+{
+	Q_OBJECT
+	Q_CLASSINFO("name", "lidar")
+	Q_CLASSINFO("friendlyName", tr("Lidar"))
+	Q_CLASSINFO("simulated", "true")
+
+public:
+	/// Constructor, takes device type info and port on which this sensor is configured.
+	LidarSensor(const DeviceInfo &info, const PortInfo &port);
+};
+
+}
+}
+}

plugins/robots/common/kitBase/kitBase.pri

@@ -94,6 +94,7 @@ HEADERS += \
 	$$PWD/include/kitBase/robotModel/robotParts/vectorSensor.h \
 	$$PWD/include/kitBase/robotModel/robotParts/shell.h \
 	$$PWD/include/kitBase/robotModel/robotParts/motorsAggregator.h \
+	$$PWD/include/kitBase/robotModel/robotParts/lidarSensor.h \
 
 SOURCES += \
 	$$PWD/src/devicesConfigurationProvider.cpp \
@@ -151,3 +152,4 @@ SOURCES += \
 	$$PWD/src/robotModel/robotParts/vectorSensor.cpp \
 	$$PWD/src/robotModel/robotParts/shell.cpp \
 	$$PWD/src/robotModel/robotParts/motorsAggregator.cpp \
+	$$PWD/src/robotModel/robotParts/lidarSensor.cpp \

plugins/robots/common/kitBase/src/robotModel/robotParts/lidarSensor.cpp

@@ -0,0 +1,23 @@
+/* Copyright 2021 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.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#include "kitBase/robotModel/robotParts/lidarSensor.h"
+
+using namespace kitBase::robotModel;
+using namespace robotParts;
+
+LidarSensor::LidarSensor(const DeviceInfo &info, const PortInfo &port)
+	: VectorSensor(info, port)
+{
+}

plugins/robots/common/trikKit/include/trikKit/robotModel/trikRobotModelBase.h

@@ -51,7 +51,7 @@ class TrikRobotModelBase : public kitBase::robotModel::CommonRobotModel
 	virtual kitBase::robotModel::DeviceInfo touchSensorInfo() const;
 	virtual kitBase::robotModel::DeviceInfo lightSensorInfo() const;
 	virtual kitBase::robotModel::DeviceInfo infraredSensorInfo() const;
-
+	virtual kitBase::robotModel::DeviceInfo lidarSensorInfo() const;
 	virtual kitBase::robotModel::DeviceInfo sonarSensorInfo() const;
 
 	virtual kitBase::robotModel::DeviceInfo motionSensorInfo() const;
@@ -74,6 +74,7 @@ class TrikRobotModelBase : public kitBase::robotModel::CommonRobotModel
 	virtual kitBase::robotModel::DeviceInfo gamepadConnectionIndicatorInfo() const;
 
 	virtual kitBase::robotModel::PortInfo video2Port() const;
+	virtual kitBase::robotModel::PortInfo lidarPort() const;
 };
 
 }

plugins/robots/common/trikKit/src/blocks/details/readLidarBlock.cpp

@@ -0,0 +1,32 @@
+/* Copyright 2021 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.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#include "readLidarBlock.h"
+
+using namespace trik::blocks::details;
+using namespace kitBase::robotModel;
+
+ReadLidarBlock::ReadLidarBlock(RobotModelInterface &robotModel)
+	: kitBase::blocksBase::common::DeviceBlock<robotParts::LidarSensor>(robotModel)
+{
+}
+
+void ReadLidarBlock::doJob(robotParts::LidarSensor &lidar)
+{
+	Q_UNUSED(lidar)
+	evalCode(stringProperty("variable") + " = lidar");
+	if (!errorsOccured()) {
+		emit done(mNextBlockId);
+	}
+}

plugins/robots/common/trikKit/src/blocks/details/readLidarBlock.h

@@ -0,0 +1,37 @@
+/* Copyright 2021 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.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#pragma once
+
+#include <kitBase/blocksBase/common/deviceBlock.h>
+#include <kitBase/robotModel/robotParts/lidarSensor.h>
+namespace trik {
+namespace blocks {
+namespace details {
+
+/// An interpreter`s implementation for Read Lidar block.
+class ReadLidarBlock : public kitBase::blocksBase::common::DeviceBlock<kitBase::robotModel::robotParts::LidarSensor>
+{
+	Q_OBJECT
+
+public:
+	explicit ReadLidarBlock(kitBase::robotModel::RobotModelInterface &robotModel);
+
+private:
+	void doJob(kitBase::robotModel::robotParts::LidarSensor &lidar) override;
+};
+
+}
+}
+}

plugins/robots/common/trikKit/src/blocks/trikBlocksFactoryBase.cpp

@@ -50,6 +50,7 @@
 #include "details/ledBlock.h"
 #include "details/sayBlock.h"
 #include "details/systemCommandBlock.h"
+#include "details/readLidarBlock.h"
 
 #include "details/waitGamepadButtonBlock.h"
 #include "details/waitGamepadConnectBlock.h"
@@ -166,6 +167,8 @@ qReal::interpretation::Block *TrikBlocksFactoryBase::produceBlock(const qReal::I
 
 	} else if (elementMetatypeIs(element, "TrikCalibrateGyroscope")) {
 		return new CalibrateGyroscopeBlock(mRobotModelManager->model());
+	} else if (elementMetatypeIs(element, "TrikReadLidar")) {
+		return new ReadLidarBlock(mRobotModelManager->model());
 	}
 
 	return nullptr;
@@ -194,6 +197,7 @@ qReal::IdList TrikBlocksFactoryBase::providedBlocks() const
 			<< id("TrikDetectorToVariable")
 			<< id("TrikInitVideoStreaming")
 			<< id("TrikStopVideoStreaming")
+//			<< id("TrikReadLidar")
 			<< id("TrikSendMessage")
 			<< id("TrikWaitForMessage")
 			;

plugins/robots/common/trikKit/src/robotModel/trikRobotModelBase.cpp

@@ -20,6 +20,7 @@
 #include <kitBase/robotModel/robotParts/gyroscopeSensor.h>
 #include <kitBase/robotModel/robotParts/accelerometerSensor.h>
 #include <kitBase/robotModel/robotParts/encoderSensor.h>
+#include <kitBase/robotModel/robotParts/lidarSensor.h>
 
 #include "trikKit/robotModel/parts/trikLightSensor.h"
 #include "trikKit/robotModel/parts/trikTouchSensor.h"
@@ -92,6 +93,7 @@ TrikRobotModelBase::TrikRobotModelBase(const QString &kitId, const QString &robo
 			, PortInfo::ReservedVariableType::vector), { lineSensorInfo() });
 
 	addAllowedConnection(video2Port(), { videoCameraInfo() });
+	addAllowedConnection(lidarPort(), { lidarSensorInfo() });
 
 	addAllowedConnection(PortInfo("ObjectSensorXPort", input, {}, "objectSensorX"), { objectSensorInfo() });
 	addAllowedConnection(PortInfo("ObjectSensorYPort", input, {}, "objectSensorY"), { objectSensorInfo() });
@@ -131,8 +133,8 @@ QList<PortInfo> TrikRobotModelBase::configurablePorts() const
 			, PortInfo("D2", input, {}, "sensorD2")
 			};
 
-	QList<PortInfo> const videoPorts = {video2Port()};
-	return CommonRobotModel::configurablePorts() + digitalPorts + videoPorts;
+	QList<PortInfo> const additionalPorts = {video2Port(), lidarPort()};
+	return CommonRobotModel::configurablePorts() + digitalPorts + additionalPorts;
 }
 
 QList<DeviceInfo> TrikRobotModelBase::convertibleBases() const
@@ -144,6 +146,7 @@ QList<DeviceInfo> TrikRobotModelBase::convertibleBases() const
 		, DeviceInfo::create<parts::TrikMotionSensor>()
 		, DeviceInfo::create<parts::TrikLineSensor>()
 		, DeviceInfo::create<parts::TrikVideoCamera>()
+		, DeviceInfo::create<robotParts::LidarSensor>()
 	};
 }
 
@@ -212,6 +215,11 @@ DeviceInfo TrikRobotModelBase::gyroscopeInfo() const
 	return DeviceInfo::create<robotParts::GyroscopeSensor>();
 }
 
+DeviceInfo TrikRobotModelBase::lidarSensorInfo() const
+{
+	return DeviceInfo::create<robotParts::LidarSensor>();
+}
+
 DeviceInfo TrikRobotModelBase::accelerometerInfo() const
 {
 	return DeviceInfo::create<robotParts::AccelerometerSensor>();
@@ -287,3 +295,7 @@ QHash<QString, int> TrikRobotModelBase::buttonCodes() const
 PortInfo TrikRobotModelBase::video2Port() const {
 	return PortInfo("Video2Port", tr("Video 2"), input);
 }
+
+PortInfo TrikRobotModelBase::lidarPort() const {
+	return PortInfo("LidarPort", tr("Lidar"), input, {}, "lidar", PortInfo::ReservedVariableType::vector);
+}

plugins/robots/common/trikKit/trikKit.pri

@@ -80,6 +80,7 @@ HEADERS += \
 	$$PWD/src/blocks/details/removeFileBlock.h \
 	$$PWD/src/blocks/details/trikWaitForGyroscopeBlock.h \
 	$$PWD/src/blocks/details/trikPrintTextBlock.h \
+	$$PWD/src/blocks/details/readLidarBlock.h \
 
 SOURCES += \
 	$$PWD/src/robotModel/trikRobotModelBase.cpp \
@@ -137,6 +138,7 @@ SOURCES += \
 	$$PWD/src/blocks/details/removeFileBlock.cpp \
 	$$PWD/src/blocks/details/trikWaitForGyroscopeBlock.cpp \
 	$$PWD/src/blocks/details/trikPrintTextBlock.cpp \
+	$$PWD/src/blocks/details/readLidarBlock.cpp \
 
 TRANSLATIONS += \
 	$$PWD/../../../../qrtranslations/ru/plugins/robots/trikKit_ru.ts \

plugins/robots/common/twoDModel/include/twoDModel/engine/model/worldModel.h

@@ -59,6 +59,10 @@ class TWO_D_MODEL_EXPORT WorldModel : public QObject
 	/// Measures the distance between robot and wall
 	Q_INVOKABLE int rangeReading(const QPointF &position, qreal direction, int maxDistance, qreal maxAngle) const;
 
+	/// Measures the distance between robot and solid object for each angle in maxAngle and maxDistance scanning region
+	Q_INVOKABLE QVector<int> lidarReading(const QPointF &position, qreal direction
+			, int maxDistance, qreal maxAngle) const;
+
 	/// Returns area which is seen by sonar sensor.
 	QPainterPath rangeSensorScanningRegion(const QPointF &position, qreal direction,
 			QPair<qreal,int> angleAndRange) const;

plugins/robots/common/twoDModel/include/twoDModel/engine/twoDModelEngineInterface.h

@@ -53,6 +53,12 @@ class TwoDModelEngineInterface
 	virtual int readRangeSensor(const kitBase::robotModel::PortInfo &port
 			, int maxDistance, qreal scanningAngle) const = 0;
 
+	/// Returns the array of distance values scanned by the lidar sensor.
+	/// @returns The distance in cm till the closest object object in the scanning one degree sector or -1 if no such
+	/// for each angle from 0 to scanningAngle clockwise
+	virtual QVector<int> readLidarSensor(const kitBase::robotModel::PortInfo &port
+			, int maxDistance, qreal scanningAngle) const = 0;
+
 	/// Returns 3 integer values that represents acceleration on three coordinate axes
 	virtual QVector<int> readAccelerometerSensor() const = 0;
 

plugins/robots/common/twoDModel/include/twoDModel/robotModel/parts/lidar.h

@@ -0,0 +1,52 @@
+/* Copyright 2021 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.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#pragma once
+
+#include <kitBase/robotModel/robotParts/lidarSensor.h>
+
+#include "twoDModel/twoDModelDeclSpec.h"
+
+namespace twoDModel {
+
+namespace engine {
+class TwoDModelEngineInterface;
+}
+
+namespace robotModel {
+namespace parts {
+
+/// 2D-model simulation of lidar.
+/// Configuration is perfomed immediately, the answer is ready immediately too.
+class TWO_D_MODEL_EXPORT Lidar : public kitBase::robotModel::robotParts::LidarSensor
+{
+	Q_OBJECT
+
+public:
+	Lidar(const kitBase::robotModel::DeviceInfo &info
+			, const kitBase::robotModel::PortInfo &port
+			, engine::TwoDModelEngineInterface &engine
+			, QPair<qreal, int> angleAndRange);
+
+	void read() override;
+
+protected:
+	engine::TwoDModelEngineInterface &mEngine;
+	qreal mAngle;
+	int mRange;
+};
+
+}
+}
+}

plugins/robots/common/twoDModel/src/engine/model/sensorsConfiguration.cpp

@@ -18,6 +18,7 @@
 #include <qrutils/mathUtils/geometry.h>
 
 #include "twoDModel/engine/model/sensorsConfiguration.h"
+#include "kitBase/robotModel/robotParts/lidarSensor.h"
 
 using namespace twoDModel::model;
 using namespace kitBase::robotModel;
@@ -52,9 +53,9 @@ void SensorsConfiguration::onDeviceConfigurationChanged(const QString &robotId
 QPointF SensorsConfiguration::defaultPosition(const DeviceInfo &device) const
 {
 	/// @todo: Move it somewhere?
-	return device.simulated()
-			? QPointF(mRobotSize.width() * 3 / 2, mRobotSize.height() / 2)
-			: QPointF(mRobotSize.width() / 2, mRobotSize.height() / 2);
+	return  !device.simulated() || device.isA<kitBase::robotModel::robotParts::LidarSensor>()
+			? QPointF(mRobotSize.width() / 2, mRobotSize.height() / 2)
+			: QPointF(mRobotSize.width() * 3 / 2, mRobotSize.height() / 2);
 }
 
 QPointF SensorsConfiguration::position(const PortInfo &port) const

plugins/robots/common/twoDModel/src/engine/model/worldModel.cpp

@@ -63,6 +63,29 @@ qreal WorldModel::pixelsInCm() const
 	return twoDModel::pixelsInCm;
 }
 
+QVector<int> WorldModel::lidarReading(const QPointF &position, qreal direction, int maxDistance, qreal maxAngle) const
+{
+	QVector<int> res;
+	const auto solidItemsPath = buildSolidItemsPath();
+	auto angleAndRange = QPair<qreal, int>(1, maxDistance);
+	for (int i = 0; i < maxAngle; i++) {
+		auto laserPath = rangeSensorScanningRegion(position, direction + i, angleAndRange);
+		const auto intersection = solidItemsPath.intersected(laserPath);
+		int currentRangeInCm = INT_MAX;
+		for (int j = 0; j < intersection.elementCount(); j++) {
+			auto el = intersection.elementAt(j);
+			if (el.type != QPainterPath::CurveToDataElement) {
+				auto lenght = QLineF(position, QPointF(el)).length() / pixelsInCm();
+				if (lenght < currentRangeInCm) {
+					currentRangeInCm = static_cast<int>(lenght);
+				}
+			}
+		}
+		res.append(currentRangeInCm <= maxDistance ? currentRangeInCm : 0);
+	}
+	return res;
+}
+
 int WorldModel::rangeReading(const QPointF &position, qreal direction, int maxDistance, qreal maxAngle) const
 {
 	int maxRangeCms = maxDistance;