Lidar

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;

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"
@@ -51,6 +52,7 @@ TrikRobotModelBase::TrikRobotModelBase(const QString &kitId, const QString &robo
 		lightSensorInfo()
 		, infraredSensorInfo()
 		, touchSensorInfo()
+		, lidarSensorInfo()
 	};
 
 	addAllowedConnection(PortInfo("DisplayPort", output), { displayInfo() });
@@ -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>();

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 -scanningAngle/2 to scanningAngle/2 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/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 - maxAngle/2 + 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 : -1); // maxDistance or -1 for empty result?
+	}
+	return res;
+}
+
 int WorldModel::rangeReading(const QPointF &position, qreal direction, int maxDistance, qreal maxAngle) const
 {
 	int maxRangeCms = maxDistance;

plugins/robots/common/twoDModel/src/engine/twoDModelEngineApi.cpp

@@ -119,6 +119,24 @@ int TwoDModelEngineApi::readRangeSensor(const PortInfo &port, int maxDistance, q
 	return mModel.settings().realisticSensors() ? spoilRangeReading(res) : res;
 }
 
+QVector<int> TwoDModelEngineApi::readLidarSensor(const PortInfo &port, int maxDistance, qreal scanningAngle) const
+{
+	QPair<QPointF, qreal> neededPosDir = countPositionAndDirection(port);
+
+	QVector<int> res;
+	auto && target = &mModel.worldModel();
+	QMetaObject::invokeMethod(target, [&](){res = target->lidarReading(
+	neededPosDir.first, neededPosDir.second, maxDistance, scanningAngle);}
+	, QThread::currentThread() != target->thread() ? Qt::BlockingQueuedConnection : Qt::DirectConnection);
+
+	if (mModel.settings().realisticSensors()) {
+		for (int i = 0; i < res.size(); i++) {
+			res[i] = spoilRangeReading(res[i]);
+		}
+	}
+	return res;
+}
+
 QVector<int> TwoDModelEngineApi::readAccelerometerSensor() const
 {
 	QVector<int> t;

plugins/robots/common/twoDModel/src/engine/twoDModelEngineApi.h

@@ -45,6 +45,8 @@ class TwoDModelEngineApi : public engine::TwoDModelEngineInterface
 	int readTouchSensor(const kitBase::robotModel::PortInfo &port) const override;
 	int readRangeSensor(const kitBase::robotModel::PortInfo &port
 			, int maxDistance, qreal scanningAngle) const override;
+	QVector<int> readLidarSensor(const kitBase::robotModel::PortInfo &port
+			, int maxDistance, qreal scanningAngle) const override;
 	QVector<int> readAccelerometerSensor() const override;
 	QVector<int> readGyroscopeSensor() const override;
 	QVector<int> calibrateGyroscopeSensor() override;

plugins/robots/common/twoDModel/src/engine/view/scene/sensorItem.cpp

@@ -20,6 +20,7 @@
 #include <kitBase/robotModel/robotParts/touchSensor.h>
 #include <kitBase/robotModel/robotParts/rangeSensor.h>
 #include <kitBase/robotModel/robotParts/lightSensor.h>
+#include <kitBase/robotModel/robotParts/lidarSensor.h>
 
 #include "twoDModel/robotModel/parts/colorSensorFull.h"
 #include "twoDModel/robotModel/parts/colorSensorPassive.h"
@@ -171,6 +172,8 @@ QString SensorItem::name() const
 		return "color_blue";
 	} else if (sensor.isA<robotParts::RangeSensor>()) {
 		return "range";
+	} else if (sensor.isA<robotParts::LidarSensor>()) {
+		return "range";
 	} else if (sensor.isA<robotParts::LightSensor>()
 		|| sensor.isA<robotParts::ColorSensorAmbient>()
 		|| sensor.isA<robotParts::ColorSensorReflected>()) {
@@ -191,7 +194,8 @@ QRectF SensorItem::calculateImageRect() const
 			|| sensor.isA<robotParts::LightSensor>()) {
 		return QRectF(-6, -6, 12, 12);
 	}
-	if (sensor.isA<robotParts::RangeSensor>()) {
+	if (sensor.isA<robotParts::RangeSensor>()
+			|| sensor.isA<robotParts::LidarSensor>()) {
 		return QRectF(-20, -10, 40, 20);
 	} else {
 		Q_ASSERT(!"Unknown sensor type");

plugins/robots/common/twoDModel/src/engine/view/scene/twoDModelScene.cpp

@@ -32,6 +32,7 @@
 #include <kitBase/robotModel/robotParts/lightSensor.h>
 #include <kitBase/robotModel/robotParts/rangeSensor.h>
 #include <kitBase/robotModel/robotParts/vectorSensor.h>
+#include <kitBase/robotModel/robotParts/lidarSensor.h>
 
 #include "robotItem.h"
 
@@ -996,6 +997,7 @@ void TwoDModelScene::reinitSensor(RobotItem *robotItem, const kitBase::robotMode
 	}
 
 	SensorItem *sensor = device.isA<kitBase::robotModel::robotParts::RangeSensor>()
+			|| device.isA<kitBase::robotModel::robotParts::LidarSensor>()
 			? new RangeSensorItem(mModel.worldModel(), robotModel.configuration()
 					, port
 					, robotModel.info().rangeSensorAngleAndDistance(device)

plugins/robots/common/twoDModel/src/robotModel/parts/lidar.cpp

@@ -0,0 +1,35 @@
+/* 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 "twoDModel/robotModel/parts/lidar.h"
+
+#include "twoDModel/engine/twoDModelEngineInterface.h"
+
+using namespace twoDModel::robotModel::parts;
+using namespace kitBase::robotModel;
+
+Lidar::Lidar(const DeviceInfo &info, const PortInfo &port
+		, engine::TwoDModelEngineInterface &engine, QPair<qreal, int> angleAndRange)
+	: robotParts::LidarSensor(info, port)
+	, mEngine(engine)
+	, mAngle(angleAndRange.first)
+	, mRange(angleAndRange.second)
+{
+	setLastData(QVector<int>(360, -1));
+}
+
+void Lidar::read()
+{
+	setLastData(mEngine.readLidarSensor(port(), mRange, mAngle));
+}

plugins/robots/common/twoDModel/src/robotModel/twoDRobotModel.cpp

@@ -34,6 +34,7 @@
 #include "twoDModel/robotModel/parts/gyroscope.h"
 #include "twoDModel/robotModel/parts/accelerometer.h"
 #include "twoDModel/robotModel/parts/marker.h"
+#include "twoDModel/robotModel/parts/lidar.h"
 
 #include "twoDModel/engine/twoDModelEngineInterface.h"
 
@@ -215,6 +216,10 @@ robotParts::Device *TwoDRobotModel::createDevice(const PortInfo &port, const Dev
 		return new parts::Marker(deviceInfo, port, *mEngine);
 	}
 
+	if (deviceInfo.isA<robotParts::LidarSensor>()) {
+		return new parts::Lidar(deviceInfo, port, *mEngine, rangeSensorAngleAndDistance(deviceInfo));
+	}
+
 	return CommonRobotModel::createDevice(port, deviceInfo);
 }
 

plugins/robots/common/twoDModel/twoDModel.pri

@@ -62,6 +62,7 @@ HEADERS += \
 	$$PWD/include/twoDModel/robotModel/parts/gyroscope.h \
 	$$PWD/include/twoDModel/robotModel/parts/accelerometer.h \
 	$$PWD/include/twoDModel/robotModel/parts/marker.h \
+	$$PWD/include/twoDModel/robotModel/parts/lidar.h \
 	$$PWD/include/twoDModel/blocks/markerDownBlock.h \
 	$$PWD/include/twoDModel/blocks/markerUpBlock.h \
 
@@ -211,6 +212,7 @@ SOURCES += \
 	$$PWD/src/robotModel/parts/gyroscope.cpp \
 	$$PWD/src/robotModel/parts/accelerometer.cpp \
 	$$PWD/src/robotModel/parts/marker.cpp \
+	$$PWD/src/robotModel/parts/lidar.cpp \
 	$$PWD/src/blocks/markerDownBlock.cpp \
 	$$PWD/src/blocks/markerUpBlock.cpp \
 	$$PWD/src/engine/model/physics/box2DPhysicsEngine.cpp \

plugins/robots/generators/trik/trikPythonGeneratorLibrary/templates.qrc

@@ -232,5 +232,6 @@
         <file>templates/wait/gyroscope.t</file>
         <file>templates/markerDown.t</file>
         <file>templates/markerUp.t</file>
+        <file>templates/sensors/lidar.t</file>
     </qresource>
 </RCC>

plugins/robots/generators/trik/trikPythonGeneratorLibrary/templates/sensors/lidar.t

@@ -0,0 +1 @@
+brick.lidar("@@PORT@@").read()

plugins/robots/generators/trik/trikQtsGeneratorLibrary/templates.qrc

@@ -237,5 +237,6 @@
         <file>templates/wait/gyroscope.t</file>
         <file>templates/markerDown.t</file>
         <file>templates/markerUp.t</file>
+        <file>templates/sensors/lidar.t</file>
     </qresource>
 </RCC>

plugins/robots/generators/trik/trikQtsGeneratorLibrary/templates/sensors/lidar.t

@@ -0,0 +1 @@
+brick.lidar(@@PORT@@).read()