Trik Studio 3D Visualization

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

@@ -4,7 +4,7 @@
  * 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
+ *	 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,
@@ -25,6 +25,8 @@
 
 #include "twoDModel/twoDModelDeclSpec.h"
 
+#include "plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.h"
+
 class QGraphicsItem;
 
 namespace twoDModel {
@@ -116,7 +118,7 @@ class TWO_D_MODEL_EXPORT RobotModel : public QObject
 
 	bool isRiding() const;
 
-	void serialize(QDomElement &parent) const;
+	QDomElement serialize(QDomElement &parent) const;
 	void serializeWorldModel(QDomElement &parent) const;
 	void deserialize(const QDomElement &robotElement);
 	void deserializeWorldModel(const QDomElement &world);
@@ -187,6 +189,19 @@ public slots:
 	/// Emitted when left or right wheel was reconnected to another port.
 	void wheelOnPortChanged(WheelEnum wheel, const kitBase::robotModel::PortInfo &port);
 
+	/// Emitted when robot starts or ends play beep sound
+	void trajectorySoundStateChanged(QString id, int time);
+
+	/// Emitted when robot starts or ends draw
+	void trajectoryMarkerColorChanged(QString id, QColor color);
+
+	/// Emitted when robot position and/or rotation changed
+	void trajectoryPosOrAngleChanged(QString id, QPointF position, qreal rotation);
+	void trajectoryOnitemDragged(QString id, QPointF position, qreal rotation);
+
+	void trajectorySave();
+	void onStopPlaying();
+
 private:
 	QVector2D robotDirectionVector() const;
 

plugins/robots/common/twoDModel/include/twoDModel/engine/view/twoDModelWidget.h

@@ -28,6 +28,7 @@
 #include "twoDModel/engine/twoDModelDisplayWidget.h"
 
 #include "twoDModel/twoDModelDeclSpec.h"
+#include "plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVizualizator.h"
 
 class QComboBox;
 class QPushButton;
@@ -108,6 +109,7 @@ class TWO_D_MODEL_EXPORT TwoDModelWidget : public QWidget
 	QDomDocument generateWorldModelWithBlobsXml() const;
 	QDomDocument generateWorldModelXml() const;
 	QDomDocument generateBlobsXml() const;
+	ConnectionToVizualizator *mConnToVizualizator;
 
 public slots:
 	void zoomIn() override;
@@ -129,6 +131,9 @@ public slots:
 	/// Emitted when user has stopped intepretation from the 2D model window.
 	void stopButtonPressed();
 
+	/// Emitted when user has stopped intepretation from the remote vizualizator
+	void restartRequested();
+
 protected:
 	void changeEvent(QEvent *e) override;
 	void showEvent(QShowEvent *e) override;

plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.cpp

@@ -4,7 +4,7 @@
  * 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
+ *	 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,
@@ -32,6 +32,7 @@
 #include "parts/box2DItem.h"
 
 using namespace twoDModel::model::physics;
+
 using namespace parts;
 using namespace mathUtils;
 
@@ -44,6 +45,7 @@ Box2DPhysicsEngine::Box2DPhysicsEngine (const WorldModel &worldModel
 	, mWorld(new b2World(b2Vec2(0, 0)))
 	, mPrevPosition(b2Vec2(0, 0))
 	, mPrevAngle(0)
+	, trajSaver(new TrajectorySaver(nullptr))
 {
 	connect(&worldModel, &model::WorldModel::wallAdded,
 			this, [this](const QSharedPointer<QGraphicsItem> &i) {itemAdded(i.data());});
@@ -53,6 +55,10 @@ Box2DPhysicsEngine::Box2DPhysicsEngine (const WorldModel &worldModel
 			this, [this](const QSharedPointer<QGraphicsItem> &i) {itemAdded(i.data());});
 	connect(&worldModel, &model::WorldModel::itemRemoved,
 			this, [this](const QSharedPointer<QGraphicsItem> &i) {itemRemoved(i.data());});
+
+	connect(this, &Box2DPhysicsEngine::trajectoryPosOrAngleChanged, trajSaver, &TrajectorySaver::saveItemPosOrAngle);
+	connect(this, &Box2DPhysicsEngine::trajectoryItemDragged, trajSaver, &TrajectorySaver::onItemDragged);
+	connect(this, &Box2DPhysicsEngine::sendNextFrame, trajSaver, &TrajectorySaver::sendFrame);
 }
 
 Box2DPhysicsEngine::~Box2DPhysicsEngine(){
@@ -145,6 +151,16 @@ void Box2DPhysicsEngine::addRobot(model::RobotModel * const robot)
 		});
 
 		connect(robot, &model::RobotModel::deserialized, this, &Box2DPhysicsEngine::onMouseReleased);
+
+
+		connect(robot, &RobotModel::trajectorySoundStateChanged, trajSaver, &TrajectorySaver::saveBeepState);
+		connect(robot, &RobotModel::trajectoryMarkerColorChanged, trajSaver, &TrajectorySaver::saveMarkerState);
+		connect(robot, &RobotModel::trajectoryPosOrAngleChanged, trajSaver, &TrajectorySaver::saveItemPosOrAngle);
+		connect(robot, &RobotModel::trajectoryOnitemDragged, trajSaver, &TrajectorySaver::onItemDragged);
+
+		connect(robot, &RobotModel::trajectorySave, trajSaver, &TrajectorySaver::saveToFile);
+//		connect(robot, &RobotModel::OnStartPlaying, trajSaver, &TrajectorySaver::OnStartInterpretation);
+		connect(robot, &RobotModel::onStopPlaying, trajSaver, &TrajectorySaver::onStopInterpretation);
 	});
 }
 
@@ -353,7 +369,10 @@ void Box2DPhysicsEngine::nextFrame()
 			item->setPos(scenePos - item->boundingRect().center());
 			item->setRotation(angleToScene(mBox2DDynamicItems[item]->getRotation()));
 		}
+		auto *abstractItem = dynamic_cast<graphicsUtils::AbstractItem *>(item);
+		emit trajectoryPosOrAngleChanged(abstractItem->id(), abstractItem->pos(), abstractItem->rotation());
 	}
+	emit sendNextFrame();
 }
 
 void Box2DPhysicsEngine::clearForcesAndStop()
@@ -364,6 +383,7 @@ void Box2DPhysicsEngine::clearForcesAndStop()
 		body->SetLinearVelocity({0, 0});
 		body->SetAngularVelocity(0);
 	}
+	//emit trajectorySave();
 }
 
 bool Box2DPhysicsEngine::isRobotStuck() const
@@ -428,6 +448,7 @@ void Box2DPhysicsEngine::onItemDragged(graphicsUtils::AbstractItem *item)
 				bItem->setRotation(0);
 				bItem->moveToPosition(positionToBox2D(localScenePos + localCenter));
 				bItem->setRotation(angleToBox2D(localRotation));
+				emit trajectoryItemDragged(item->id(), item->pos(), item->rotation());
 			}
 		} else {
 			b2Vec2 pos = positionToBox2D(collidingPolygon.boundingRect().center());

plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.h

@@ -4,7 +4,7 @@
  * 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
+ *	 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,
@@ -19,6 +19,7 @@
 #include <qrutils/mathUtils/geometry.h>
 
 #include "twoDModel/engine/model/worldModel.h"
+#include "plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.h"
 
 class b2World;
 class b2Body;
@@ -44,6 +45,7 @@ namespace twoDModel {
 
 class Box2DPhysicsEngine : public PhysicsEngineBase
 {
+	Q_OBJECT
 public:
 	Box2DPhysicsEngine(const WorldModel &worldModel, const QList<RobotModel *> &robots);
 	~Box2DPhysicsEngine();
@@ -84,6 +86,12 @@ public slots:
 	void onMousePressed();
 	void onRecoverRobotPosition(const QPointF &pos);
 
+	signals:
+	void trajectoryPosOrAngleChanged(QString id, QPointF pos, qreal rotation);
+	void trajectoryItemDragged(QString id, QPointF pos, qreal rotation);
+	void trajectorySave();
+	void sendNextFrame();
+
 protected:
 	void onPixelsInCmChanged(qreal value) override;
 	void itemAdded(QGraphicsItem *item) override;
@@ -104,9 +112,11 @@ public slots:
 	QMap<QGraphicsItem *, parts::Box2DItem *> mBox2DResizableItems;  // Takes ownership on b2Body instances
 	QMap<QGraphicsItem *, parts::Box2DItem *> mBox2DDynamicItems;  // Doesn't take ownership
 	QMap<RobotModel *, QSet<twoDModel::view::SensorItem *>> mRobotSensors; // Doesn't take ownership
+	QList<parts::Box2DItem *> inMoveItems;
 
 	b2Vec2 mPrevPosition;
 	float mPrevAngle;
+	TrajectorySaver const *trajSaver {};
 };
 
 }

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

@@ -4,7 +4,7 @@
  * 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
+ *	 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,
@@ -68,6 +68,7 @@ void RobotModel::reinit()
 	mBeepTime = 0;
 	mDeltaDegreesOfAngle = 0;
 	mAcceleration = QPointF(0, 0);
+	//emit OnStartPlaying();
 }
 
 void RobotModel::clear()
@@ -94,8 +95,8 @@ RobotModel::Wheel *RobotModel::initMotor(int radius, int speed, uint64_t degrees
 	mMotors[port].reset(motor);
 
 	/// @todo We need some mechanism to set correspondence between motors and encoders. In NXT motors and encoders are
-	///       physically plugged into one port, so we can find corresponding port by name. But in TRIK encoders can be
-	///       connected differently.
+	///	   physically plugged into one port, so we can find corresponding port by name. But in TRIK encoders can be
+	///	   connected differently.
 	for (const Device * const device : mRobotModel.configuration().devices()) {
 		if (device->deviceInfo().isA<EncoderSensor>()
 				&& (device->port().name() == port.name() || device->port().nameAliases().contains(port.name())))
@@ -111,6 +112,7 @@ RobotModel::Wheel *RobotModel::initMotor(int radius, int speed, uint64_t degrees
 void RobotModel::playSound(int timeInMs)
 {
 	mBeepTime = qMax(mBeepTime, timeInMs);
+	emit trajectorySoundStateChanged(mRobotModel.robotId(), mBeepTime);
 }
 
 void RobotModel::setNewMotor(int speed, uint degrees, const PortInfo &port, bool breakMode)
@@ -179,6 +181,8 @@ void RobotModel::stopRobot()
 	mIsFirstAngleStamp = true;
 	mPosStamps.clear();
 	emit playingSoundChanged(false);
+	emit onStopPlaying();
+	emit trajectorySave();
 	for (auto &&engine : mMotors) {
 		engine->speed = 0;
 		engine->breakMode = true;
@@ -251,9 +255,9 @@ QPainterPath RobotModel::sensorBoundingPath(const PortInfo &port) const
 	QPainterPath tempSensorPath;
 	tempSensorPath.addRect(sensorRect(port, sensorPos));
 	const QTransform transformSensor = QTransform()
-			.translate(sensorPos.x(), sensorPos.y())        // /\  And going back again
+			.translate(sensorPos.x(), sensorPos.y())		// /\  And going back again
 			.rotate(mSensorsConfiguration.direction(port))  // ||  Then rotating
-			.translate(-sensorPos.x(), -sensorPos.y());     // ||  First translating to zero
+			.translate(-sensorPos.x(), -sensorPos.y());	 // ||  First translating to zero
 	return transformSensor.map(tempSensorPath);
 }
 
@@ -301,11 +305,13 @@ QColor RobotModel::markerColor() const
 void RobotModel::markerDown(const QColor &color)
 {
 	mMarker = color;
+	emit trajectoryMarkerColorChanged(mRobotModel.robotId(), color);
 }
 
 void RobotModel::markerUp()
 {
 	mMarker = Qt::transparent;
+	emit trajectoryMarkerColorChanged(mRobotModel.robotId(), Qt::transparent);
 }
 
 QVector<int> RobotModel::accelerometerReading() const
@@ -372,6 +378,10 @@ void RobotModel::nextFragment()
 	}
 
 	emit robotRided(mPos, mAngle);
+	if (isRiding())
+	{
+		emit trajectoryPosOrAngleChanged(mRobotModel.robotId(), mPos, mAngle);
+	}
 }
 
 QPointF RobotModel::position() const
@@ -384,6 +394,7 @@ void RobotModel::setPosition(const QPointF &newPos)
 	if (newPos != mPos) {
 		mPos = newPos;
 		emit positionChanged(newPos);
+		emit trajectoryOnitemDragged(mRobotModel.robotId(), mPos, mAngle);
 	}
 }
 
@@ -397,6 +408,7 @@ void RobotModel::setRotation(qreal angle)
 	if (!mathUtils::Math::eq(mAngle, angle)) {
 		mAngle = angle;
 		emit rotationChanged(angle);
+		emit trajectoryOnitemDragged(mRobotModel.robotId(), mPos, mAngle);
 	}
 }
 
@@ -415,23 +427,19 @@ bool RobotModel::onTheGround() const
 	return mIsOnTheGround;
 }
 
-void RobotModel::serialize(QDomElement &parent) const
+QDomElement RobotModel::serialize(QDomElement &parent) const
 {
-	QDomElement curRobot = parent.ownerDocument().createElement("robot");
-	curRobot.setAttribute("id", mRobotModel.robotId());	
-	mSensorsConfiguration.serialize(curRobot);
-	serializeWheels(curRobot);
+	QDomElement robot = parent.ownerDocument().createElement("robot");
+	parent.appendChild(robot);
+	robot.setAttribute("id", mRobotModel.robotId());
+	robot.setAttribute("position", QString::number(mPos.x()) + ":" + QString::number(mPos.y()));
+	robot.setAttribute("direction", QString::number(mAngle));
 
-	bool replaced = false;
-	for (QDomElement robot = parent.firstChildElement("robot"); !robot.isNull()
-			; robot = robot.nextSiblingElement("robot")) {
-		if (robot.attribute("id") == mRobotModel.robotId()) {
-			parent.replaceChild(curRobot, robot);
-			replaced = true;
-			break;
-		}
-	}
-	if (!replaced) parent.appendChild(curRobot);
+	mSensorsConfiguration.serialize(robot);
+	mStartPositionMarker->serialize(robot);
+	serializeWheels(robot);
+
+	return robot;
 }
 
 void RobotModel::serializeWorldModel(QDomElement &parent) const
@@ -471,8 +479,16 @@ void RobotModel::deserializeWorldModel(const QDomElement &world)
 
 void RobotModel::deserialize(const QDomElement &robotElement)
 {
+	const QString positionStr = robotElement.attribute("position", "0:0");
+	const QStringList splittedStr = positionStr.split(":");
+	const qreal x = static_cast<qreal>(splittedStr[0].toDouble());
+	const qreal y = static_cast<qreal>(splittedStr[1].toDouble());
+	onRobotReturnedOnGround();
+	setPosition(QPointF(x, y));
+	setRotation(robotElement.attribute("direction", "0").toDouble());
+	mStartPositionMarker->deserializeCompatibly(robotElement);
 	deserializeWheels(robotElement);
-	configuration().deserialize(robotElement);
+	emit deserialized(QPointF(x, y), robotElement.attribute("direction", "0").toDouble());
 	nextFragment();
 }
 

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

@@ -34,7 +34,6 @@ Timeline::Timeline(QObject *parent)
 		qRegisterMetaType<qReal::interpretation::StopReason>();
 		registered = true;
 	}
-
 	connect(&mTimer, &QTimer::timeout, this, &Timeline::onTimer);
 	mTimer.setInterval(defaultRealTimeInterval);
 }