diff --git a/plugins/robots/common/twoDModel/include/twoDModel/engine/model/robotModel.h b/plugins/robots/common/twoDModel/include/twoDModel/engine/model/robotModel.h index 8fa921e93e..230ad2fce9 100644 --- a/plugins/robots/common/twoDModel/include/twoDModel/engine/model/robotModel.h +++ b/plugins/robots/common/twoDModel/include/twoDModel/engine/model/robotModel.h @@ -187,6 +187,21 @@ 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(const QString &id, const qreal time); + + /// Emitted when robot starts or ends draw + void trajectoryMarkerColorChanged(const QString &id, const QColor &color); + + /// Emitted when robot position and/or rotation changed + void trajectoryPosChanged(const QString &id, const QPointF &position); + void trajectoryRotChanged(const QString &id, const qreal rotation); + void trajectoryOnitemDragged(); + void trajectoryCleanTrace(const QString &robotId); + + void trajectorySave(); + void onStopPlaying(); + private: QVector2D robotDirectionVector() const; diff --git a/plugins/robots/common/twoDModel/include/twoDModel/engine/view/twoDModelWidget.h b/plugins/robots/common/twoDModel/include/twoDModel/engine/view/twoDModelWidget.h index 0a34e36f3e..d45c88457d 100644 --- a/plugins/robots/common/twoDModel/include/twoDModel/engine/view/twoDModelWidget.h +++ b/plugins/robots/common/twoDModel/include/twoDModel/engine/view/twoDModelWidget.h @@ -47,6 +47,10 @@ class ControllerInterface; namespace twoDModel { +namespace trajectory { +class ConnectionToVisualizer; +} + namespace model { class Model; class RobotModel; @@ -101,6 +105,9 @@ class TWO_D_MODEL_EXPORT TwoDModelWidget : public QWidget void setBackgroundMode(); + /// Returns a reference to a connection to vizualizer tool + twoDModel::trajectory::ConnectionToVisualizer *connToVisualizer(); + QString editorId() const override; bool supportsZooming() const override; void configure(QAction &zoomIn, QAction &zoomOut, QAction &undo, QAction &redo, QAction ©, QAction &paste @@ -129,6 +136,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 visualizator + void restartRequested(); + protected: void changeEvent(QEvent *e) override; void showEvent(QShowEvent *e) override; @@ -244,6 +254,7 @@ private slots: RobotItem *mSelectedRobotItem {}; kitBase::DevicesConfigurationWidget *mCurrentConfigurer {}; + twoDModel::trajectory::ConnectionToVisualizer *mConnToVisualizer {}; // Takes ownership model::Model &mModel; qReal::ControllerInterface *mController {}; diff --git a/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.cpp b/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.cpp index 275ec89079..a48ed28477 100644 --- a/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.cpp +++ b/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.cpp @@ -17,6 +17,7 @@ #include #include "twoDModel/engine/model/robotModel.h" +#include "src/engine/trajectory/trajectorySaver.h" #include "twoDModel/engine/model/constants.h" #include "twoDModel/engine/model/worldModel.h" #include "src/engine/view/scene/twoDModelScene.h" @@ -44,6 +45,7 @@ Box2DPhysicsEngine::Box2DPhysicsEngine (const WorldModel &worldModel , mWorld(new b2World(b2Vec2(0, 0))) , mPrevPosition(b2Vec2(0, 0)) , mPrevAngle(0) + , mTrajSaver(new trajectory::TrajectorySaver(this)) { connect(&worldModel, &model::WorldModel::wallAdded, this, [this](const QSharedPointer &i) {itemAdded(i.data());}); @@ -53,6 +55,14 @@ Box2DPhysicsEngine::Box2DPhysicsEngine (const WorldModel &worldModel this, [this](const QSharedPointer &i) {itemAdded(i.data());}); connect(&worldModel, &model::WorldModel::itemRemoved, this, [this](const QSharedPointer &i) {itemRemoved(i.data());}); + + connect(this, &Box2DPhysicsEngine::trajectoryPosChanged, + mTrajSaver, &trajectory::TrajectorySaver::saveItemPosition); + connect(this, &Box2DPhysicsEngine::trajectoryRotChanged, + mTrajSaver, &trajectory::TrajectorySaver::saveItemRotation); + connect(this, &Box2DPhysicsEngine::trajectoryItemDragged, mTrajSaver, + &trajectory::TrajectorySaver::onItemDragged); + connect(this, &Box2DPhysicsEngine::sendNextFrame, mTrajSaver, &trajectory::TrajectorySaver::sendFrame); } Box2DPhysicsEngine::~Box2DPhysicsEngine(){ @@ -145,6 +155,22 @@ void Box2DPhysicsEngine::addRobot(model::RobotModel * const robot) }); connect(robot, &model::RobotModel::deserialized, this, &Box2DPhysicsEngine::onMouseReleased); + connect(robot, &RobotModel::trajectorySoundStateChanged, + mTrajSaver, &trajectory::TrajectorySaver::saveBeepState); + connect(robot, &RobotModel::trajectoryMarkerColorChanged, + mTrajSaver, &trajectory::TrajectorySaver::saveMarkerState); + connect(robot, &RobotModel::trajectoryPosChanged, + mTrajSaver, &trajectory::TrajectorySaver::saveItemPosition); + connect(robot, &RobotModel::trajectoryRotChanged, + mTrajSaver, &trajectory::TrajectorySaver::saveItemRotation); + connect(robot, &RobotModel::trajectoryOnitemDragged, + mTrajSaver, &trajectory::TrajectorySaver::onItemDragged); + connect(robot, &RobotModel::trajectoryCleanTrace, + mTrajSaver, &trajectory::TrajectorySaver::onCleanRobotTrace); + connect(robot, &RobotModel::trajectorySave, + mTrajSaver, &trajectory::TrajectorySaver::saveToFile); + connect(robot, &RobotModel::onStopPlaying, + mTrajSaver, &trajectory::TrajectorySaver::onStopInterpretation); }); } @@ -212,6 +238,7 @@ void Box2DPhysicsEngine::onRecoverRobotPosition(const QPointF &pos) stop(mBox2DRobots.first()->getWheelAt(1)->getBody()); onMouseReleased(pos, mBox2DRobots.keys().first()->startPositionMarker()->rotation()); + mTrajSaver->onCleanRobotTrace(mBox2DRobots.keys().first()->info().robotId()); } void Box2DPhysicsEngine::removeRobot(model::RobotModel * const robot) @@ -352,8 +379,13 @@ void Box2DPhysicsEngine::nextFrame() QPointF scenePos = positionToScene(mBox2DDynamicItems[item]->getPosition()); item->setPos(scenePos - item->boundingRect().center()); item->setRotation(angleToScene(mBox2DDynamicItems[item]->getRotation())); + + auto *abstractItem = dynamic_cast(item); + emit trajectoryPosChanged(abstractItem->id(), abstractItem->pos()); + emit trajectoryRotChanged(abstractItem->id(), abstractItem->rotation()); } } + emit sendNextFrame(); } void Box2DPhysicsEngine::clearForcesAndStop() @@ -364,6 +396,7 @@ void Box2DPhysicsEngine::clearForcesAndStop() body->SetLinearVelocity({0, 0}); body->SetAngularVelocity(0); } + //emit trajectorySave(); } bool Box2DPhysicsEngine::isRobotStuck() const @@ -428,6 +461,9 @@ void Box2DPhysicsEngine::onItemDragged(graphicsUtils::AbstractItem *item) bItem->setRotation(0); bItem->moveToPosition(positionToBox2D(localScenePos + localCenter)); bItem->setRotation(angleToBox2D(localRotation)); + emit trajectoryPosChanged(item->id(), item->pos()); + emit trajectoryRotChanged(item->id(), item->rotation()); + emit trajectoryItemDragged(); } } else { b2Vec2 pos = positionToBox2D(collidingPolygon.boundingRect().center()); diff --git a/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.h b/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.h index cff802db48..f772e2e022 100644 --- a/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.h +++ b/plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.h @@ -34,6 +34,10 @@ namespace twoDModel { class SensorItem; } + namespace trajectory { + class TrajectorySaver; + } + namespace model { namespace physics { namespace parts { @@ -44,6 +48,7 @@ namespace twoDModel { class Box2DPhysicsEngine : public PhysicsEngineBase { + Q_OBJECT public: Box2DPhysicsEngine(const WorldModel &worldModel, const QList &robots); ~Box2DPhysicsEngine(); @@ -84,6 +89,13 @@ public slots: void onMousePressed(); void onRecoverRobotPosition(const QPointF &pos); + signals: + void trajectoryPosChanged(const QString &id, const QPointF &pos); + void trajectoryRotChanged(const QString &id, const qreal &rotation); + void trajectoryItemDragged(); + void trajectorySave(); + void sendNextFrame(); + protected: void onPixelsInCmChanged(qreal value) override; void itemAdded(QGraphicsItem *item) override; @@ -104,9 +116,11 @@ public slots: QMap mBox2DResizableItems; // Takes ownership on b2Body instances QMap mBox2DDynamicItems; // Doesn't take ownership QMap> mRobotSensors; // Doesn't take ownership + QList inMoveItems; b2Vec2 mPrevPosition; float mPrevAngle; + trajectory::TrajectorySaver *mTrajSaver {}; // Takes ownership }; } diff --git a/plugins/robots/common/twoDModel/src/engine/model/robotModel.cpp b/plugins/robots/common/twoDModel/src/engine/model/robotModel.cpp index b83efc1873..fd7ac62a2c 100644 --- a/plugins/robots/common/twoDModel/src/engine/model/robotModel.cpp +++ b/plugins/robots/common/twoDModel/src/engine/model/robotModel.cpp @@ -68,6 +68,7 @@ void RobotModel::reinit() mBeepTime = 0; mDeltaDegreesOfAngle = 0; mAcceleration = QPointF(0, 0); + //emit OnStartPlaying(); } void RobotModel::clear() @@ -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; @@ -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 RobotModel::accelerometerReading() const @@ -372,6 +378,11 @@ void RobotModel::nextFragment() } emit robotRided(mPos, mAngle); + if (isRiding()) + { + emit trajectoryPosChanged(mRobotModel.robotId(), mPos); + emit trajectoryRotChanged(mRobotModel.robotId(), mAngle); + } } QPointF RobotModel::position() const @@ -384,6 +395,9 @@ void RobotModel::setPosition(const QPointF &newPos) if (newPos != mPos) { mPos = newPos; emit positionChanged(newPos); + emit trajectoryCleanTrace(mRobotModel.robotId()); + emit trajectoryPosChanged(mRobotModel.robotId(), newPos); + emit trajectoryOnitemDragged(); } } @@ -397,6 +411,8 @@ void RobotModel::setRotation(qreal angle) if (!mathUtils::Math::eq(mAngle, angle)) { mAngle = angle; emit rotationChanged(angle); + emit trajectoryRotChanged(mRobotModel.robotId(), mAngle); + emit trajectoryOnitemDragged(); } } @@ -418,7 +434,7 @@ bool RobotModel::onTheGround() const void RobotModel::serialize(QDomElement &parent) const { QDomElement curRobot = parent.ownerDocument().createElement("robot"); - curRobot.setAttribute("id", mRobotModel.robotId()); + curRobot.setAttribute("id", mRobotModel.robotId()); mSensorsConfiguration.serialize(curRobot); serializeWheels(curRobot); diff --git a/plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVisualizer.cpp b/plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVisualizer.cpp new file mode 100644 index 0000000000..acd854361f --- /dev/null +++ b/plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVisualizer.cpp @@ -0,0 +1,170 @@ +/* Copyright 2022 Lada Egorova + * + * 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 +#include +#include "connectionToVisualizer.h" +#include +#include + +using namespace twoDModel::trajectory; + +/// Connection to Unity to send frames, run/stop/restart signals +ConnectionToVisualizer::ConnectionToVisualizer(QObject *parent) + : QObject(parent) +// , mPort(port) +{ + setIp(qReal::SettingsManager::value("UnityTcpServer").toString()); + mSendData = qReal::SettingsManager::value("UnitySendData").toBool(); + qReal::SettingsListener::listen("UnitySendData", [this](bool send){ + mSendData = send; + }, this); + qReal::SettingsListener::listen("UnityTcpServer", [this](){ + setIp(qReal::SettingsManager::value(("UnityTcpServer")).toString()); + if (mSendData){ + connectToHost(); + } + }, this); +} + +ConnectionToVisualizer::~ConnectionToVisualizer() +{ + reset(); + mKeepaliveTimer->deleteLater(); + mSocket->deleteLater(); +} + +void ConnectionToVisualizer::init() +{ + mKeepaliveTimer = new QTimer(); + mSocket = new QTcpSocket(); + qRegisterMetaType(); + connect(mKeepaliveTimer, &QTimer::timeout, this, [this]() { write("keepalive \n"); } ); + connect(mSocket, &QTcpSocket::readyRead, this, &ConnectionToVisualizer::onReadyRead); +} + +bool ConnectionToVisualizer::isConnected() const +{ + return mSocket->state() == QTcpSocket::ConnectedState; +} + +void ConnectionToVisualizer::write(const QString &data) +{ + int result = 0; + if (mSendData) + { + if (result == mSocket->write(data.toLatin1().data()) != 0){ + QLOG_ERROR() << "Exception occured when sending data"; + } + } +} + +void ConnectionToVisualizer::onReadyRead() +{ + const QByteArray &data = mSocket->readAll(); + mBuffer = data; + if (mBuffer == "Stop") { + emit stopRequested(); + } + else if (mBuffer == "Run") { + emit runRequested(); + } + else if (mBuffer == "Restart") { + emit restartRequested(); + } +} + +void ConnectionToVisualizer::stopPressed() +{ + write("Stop"); +} + +void ConnectionToVisualizer::startPressed() +{ + write("Run"); +} + +void ConnectionToVisualizer::restartPressed() +{ + write("Restart"); +} + +void ConnectionToVisualizer::reset() +{ + if (!mKeepaliveTimer->isActive()) + return; + mKeepaliveTimer->stop(); + mSocket->disconnectFromHost(); +} + +quint16 ConnectionToVisualizer::getPort() const +{ + return mPort; +} + +QString ConnectionToVisualizer::getIp() const +{ + return mIp; +} + +void ConnectionToVisualizer::setPort(const quint16 &value) +{ + mPort = value; +} + +void ConnectionToVisualizer::setIp(const QString &value) +{ + mIp = value; +} + +void ConnectionToVisualizer::connectToHost() +{ + if (mSendData) + { + reset(); + constexpr auto timeout = 3000; + QEventLoop loop; + QTimer::singleShot(timeout, &loop, &QEventLoop::quit); + connect(mSocket, &QTcpSocket::connected, &loop, &QEventLoop::quit); + connect(mSocket + , static_cast(&QTcpSocket::error) + , &loop + , [&loop](QAbstractSocket::SocketError) { loop.quit(); }); + mSocket->setProxy(QNetworkProxy::NoProxy); + mSocket->connectToHost(mIp, mPort); + loop.exec(); + + if (mSocket->state() == QTcpSocket::ConnectedState) { + mKeepaliveTimer->start(3000); + } else { + mSocket->abort(); + } + } +} + +void ConnectionToVisualizer::disconnectFromHost() +{ + if (isConnected()) { + mSocket->disconnectFromHost(); + if (mSocket->state() != QAbstractSocket::UnconnectedState) { + mSocket->waitForDisconnected(3000); + } + } +} + +bool ConnectionToVisualizer::isSendingData() +{ + return mSendData; +} +//} diff --git a/plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVisualizer.h b/plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVisualizer.h new file mode 100644 index 0000000000..0d918e9b3e --- /dev/null +++ b/plugins/robots/common/twoDModel/src/engine/trajectory/connectionToVisualizer.h @@ -0,0 +1,84 @@ +/* Copyright 2022 Lada Egorova + * + * 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 +#include +#include +#include +#include + +namespace twoDModel { +namespace trajectory { +/// Connection to Unity to send frames and run/stop/restart +class alignas(8) ConnectionToVisualizer : public QObject +{ + Q_OBJECT + Q_DISABLE_COPY(ConnectionToVisualizer) + +public: + explicit ConnectionToVisualizer(QObject *parent = nullptr); + ~ConnectionToVisualizer(); + + /// inits manager after moved to correct thread + void init(); + + /// checks connection + bool isConnected() const; + + /// sets ip + void setIp(const QString &value); + + /// sets port + void setPort(const quint16 &value); + + void onReadyRead(); + + bool isSendingData(); + + /// returns ip + QString getIp() const; + + /// returns port + quint16 getPort() const; + +public slots: + void connectToHost(); + void disconnectFromHost(); + void write(const QString &); + + /// Disconnect + void reset(); + + void stopPressed(); + void startPressed(); + void restartPressed(); + +signals: + void stopRequested(); + void runRequested(); + void restartRequested(); + +private: + QTcpSocket *mSocket; + QTimer *mKeepaliveTimer; + + QString mIp {"10.0.5.2"}; + quint16 mPort { 8080 }; + QByteArray mBuffer; + bool mSendData = true; +}; +} +} diff --git a/plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.cpp b/plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.cpp new file mode 100644 index 0000000000..882e11c043 --- /dev/null +++ b/plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.cpp @@ -0,0 +1,188 @@ +/* Copyright 2022 Lada Egorova + * + * 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 "trajectorySaver.h" +#include "connectionToVisualizer.h" +#include +#include +#include +#include +#include + +using namespace twoDModel::trajectory; + +TrajectorySaver::TrajectorySaver(QObject *parent) + : QObject(parent) + , mConnToVisualizer(new ConnectionToVisualizer(this)) +{ + mConnToVisualizer->setPort(8080); + mConnToVisualizer->init(); +} + +TrajectorySaver::~TrajectorySaver() +{ + mConnToVisualizer->disconnectFromHost(); +} + +void TrajectorySaver::saveBeepState(const QString &robotId, const qreal time) +{ + stringstream value; + value << "beepState=" << time; + currStates.append(createState(robotId, value.str().c_str())); +} + +void TrajectorySaver::saveMarkerState(const QString &robotId, const QColor &color) +{ + stringstream value; + value << "markerState=" << color.red() << " " << color.green() << " " << color.blue() << " " << color.alpha(); + currStates.append(createState(robotId, value.str().c_str())); +} + +void TrajectorySaver::saveItemPosition(const QString &id, const QPointF &pos) +{ + stringstream value; + value << "pos=" << pos.x() << " " << -pos.y(); + addState(id, value.str().c_str()); + isPlaying = true; +} + +void TrajectorySaver::saveItemRotation(const QString &id, const qreal rotation) +{ + stringstream value; + value << "rot=" << rotation; + addState(id, value.str().c_str()); + isPlaying = true; +} + +void TrajectorySaver::onItemDragged() +{ + if (!isPlaying) { + sendFrame(); + } +} + +/// Syntactic sugar for onItemDragged and saveItemPosOrState +void TrajectorySaver::addState(const QString &id, const QString &value) +{ + auto state = createState(id, value); + if (!currStates.contains(QJsonValue(state))) { + //qDebug(qUtf8Printable(id)); + /// to avoid duplicates, it can be because onItemDragged calls when + /// objects is moved by other object or by user + currStates.append(state); + } +} + +QJsonObject TrajectorySaver::createState(const QString &id, const QString &stateStr) +{ + QJsonObject state; + state.insert("id", id); + state.insert("state", stateStr); + return state; +} + +void TrajectorySaver::sendFrame() +{ + auto frame = saveFrame(); + QJsonDocument doc; + doc.setObject(frame); + QString data (doc.toJson( QJsonDocument::Compact)); + if (mConnToVisualizer->isSendingData()) + { + sendTrajectory(data); + } +} + +/// Appends current frame to all frames +QJsonObject TrajectorySaver::saveFrame() +{ + QJsonObject frame; + frame.insert("frame", currStates); + frames.append(frame); + /// updating current states every frame + currStates = *new QJsonArray(); + return frame; +} + +void TrajectorySaver::saveToFile() +{ + sendFrame(); + addState("endState", ""); + sendFrame(); + + /// creating json document + QJsonDocument doc; + QJsonObject root; + root.insert("frames", frames); + doc.setObject(root); + QByteArray bytes = doc.toJson( QJsonDocument::Compact ); + + /// saving it to file + QFile file("trajectory.json"); + try { + file.open( QIODevice::WriteOnly | QIODevice::Text | QIODevice::Truncate ); + QTextStream iStream( &file ); + iStream.setCodec( "utf-8" ); + iStream << bytes; + file.close(); + } + catch (const ofstream::failure& e){ + QLOG_ERROR() << "Exception occured when opening/writing to file" + << e.what(); + } +} + +void TrajectorySaver::reinitConnection() +{ + if (mConnToVisualizer == nullptr) { +// mConnToVisualizer = new ConnectionToVisualizer(); + mConnToVisualizer->setPort(8080); + mConnToVisualizer->init(); + } + mConnToVisualizer->connectToHost(); +// qDebug(qPrintable(mConnToVisualizer->getIp())); +} + +void TrajectorySaver::sendTrajectory(const QString &data = nullptr) +{ + if (mConnToVisualizer == nullptr || !mConnToVisualizer->isConnected()) { + reinitConnection(); + } + +// qDebug(qPrintable(data)); + if (data != nullptr) { + mConnToVisualizer->write(data); + } else { + QFile file("trajectory.json"); + try { + file.open(QIODevice::ReadOnly); + QString fileData = file.readAll(); + mConnToVisualizer->write(fileData); + file.close(); + } catch (const ofstream::failure& e){ + QLOG_ERROR() << "Exception occured when opening/writing to file" + << e.what(); + } + } +} + +void TrajectorySaver::onStopInterpretation() +{ + isPlaying = false; +} + +void TrajectorySaver::onCleanRobotTrace(const QString &robotId) +{ + saveMarkerState(robotId, Qt::GlobalColor::transparent); +} diff --git a/plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.h b/plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.h new file mode 100644 index 0000000000..80056fa0e7 --- /dev/null +++ b/plugins/robots/common/twoDModel/src/engine/trajectory/trajectorySaver.h @@ -0,0 +1,91 @@ +/* Copyright 2022 Lada Egorova + * + * 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 +#include + +#include +#include +#include +#include +#include + +#include +#include "connectionToVisualizer.h" +using namespace std; + +namespace twoDModel { +namespace trajectory { +/// Main class for saving trajectories of all dynamic objects on scene (robot, ball, skittle) +/// Saves trajectory as a sequence of frames. Frame is a scene state in particular moment. +/// Every frame consists of states. State describes status of a single object +class TrajectorySaver : public QObject +{ + Q_OBJECT +public: + explicit TrajectorySaver(QObject *parent = nullptr); + ~TrajectorySaver(); + +private: + QScopedPointer mConnToVisualizer; // Takes ownership + + bool isPlaying = false; + QJsonArray frames; + QJsonArray currStates; + + void sendTrajectory(const QString &data); +// void addState(const QString &id, const QPointF &pos, const qreal &rotation); + void addState(const QString &id, const QString &value); + + QJsonObject createState(const QString &id, const QString &stateStr); + QJsonObject saveFrame(); + + void reinitConnection(); + +public slots: + /// Emitted when robot/ball/skittle is moved by user. It can happen while playing or not. + void onItemDragged(); + + /// Save position and angle of robot. Emitted when nextFragment is playing, + /// save position and angle of balls/skittles. Emitted when nextFrame is playing + void saveItemPosition(const QString &id, const QPointF &pos); + void saveItemRotation(const QString &id, const qreal rotation); + // void saveItemPosOrAngle(const QString &id, const QPointF &pos, const qreal &rotation); + + /// Emitted when robot plays beep sound, saves beeping time in ms + void saveBeepState(const QString &robotId, const qreal time); + + /// Emitted when robot places marker up or down, saves marker color: + /// if marker is down, color is equal to marker color, if marker is ip, + /// color is transparent + void saveMarkerState(const QString &robotId, const QColor &color); + + /// Emitted when robot stops + void saveToFile(); + + /// Emitted when one frame is played, sends frame/set of frames + /// (depends on perfomance) to server in Json Compact format + void sendFrame(); + + /// Emitted when user presses "Stop" button in visualizator + void onStopInterpretation(); + + /// Emitted when robot was dragged or returned to start position + /// Stops drawing a line + void onCleanRobotTrace(const QString &robotId); +}; +} +} diff --git a/plugins/robots/common/twoDModel/src/engine/twoDModelEngineFacade.cpp b/plugins/robots/common/twoDModel/src/engine/twoDModelEngineFacade.cpp index 24dc3ec090..75347b992c 100644 --- a/plugins/robots/common/twoDModel/src/engine/twoDModelEngineFacade.cpp +++ b/plugins/robots/common/twoDModel/src/engine/twoDModelEngineFacade.cpp @@ -21,6 +21,7 @@ #include "twoDModel/engine/model/model.h" #include "twoDModel/engine/view/twoDModelWidget.h" #include "twoDModelEngineApi.h" +#include "src/engine/trajectory/connectionToVisualizer.h" #include @@ -187,6 +188,7 @@ void TwoDModelEngineFacade::onStartInterpretation() mModel->errorReporter()->addWarning(tr("Realistic physics' must be turned on to enjoy skittles and balls")); } mModel->timeline().start(); + mView->connToVisualizer()->connectToHost(); } void TwoDModelEngineFacade::onStopInterpretation(qReal::interpretation::StopReason reason) diff --git a/plugins/robots/common/twoDModel/src/engine/view/twoDModelWidget.cpp b/plugins/robots/common/twoDModel/src/engine/view/twoDModelWidget.cpp index 04ab29f426..7a66c5f9ea 100644 --- a/plugins/robots/common/twoDModel/src/engine/view/twoDModelWidget.cpp +++ b/plugins/robots/common/twoDModel/src/engine/view/twoDModelWidget.cpp @@ -51,6 +51,7 @@ #include "src/engine/items/startPosition.h" #include "src/engine/commands/changePropertyCommand.h" #include "src/engine/commands/loadWorldCommand.h" +#include "src/engine/trajectory/connectionToVisualizer.h" #include "twoDModel/engine/model/constants.h" #include "twoDModel/engine/model/model.h" @@ -75,6 +76,7 @@ TwoDModelWidget::TwoDModelWidget(Model &model, QWidget *parent) : QWidget(parent) , mUi(new Ui::TwoDModelWidget) , mActions(new ActionsBox) + , mConnToVisualizer(new twoDModel::trajectory::ConnectionToVisualizer(this)) , mModel(model) , mDisplay(new twoDModel::engine::NullTwoDModelDisplayWidget(this)) , mNullDisplay(new twoDModel::engine::NullTwoDModelDisplayWidget(this)) @@ -130,6 +132,20 @@ TwoDModelWidget::TwoDModelWidget(Model &model, QWidget *parent) // Setting value in percents mSpeedPopup->setSpeed(100 / speedFactors[defaultSpeedFactorIndex] * value); }); + mConnToVisualizer->setPort(9000); + mConnToVisualizer->init(); + mConnToVisualizer->connectToHost(); + connect(mConnToVisualizer, &twoDModel::trajectory::ConnectionToVisualizer::stopRequested, + this, &TwoDModelWidget::stopButtonPressed); + connect(mConnToVisualizer, &twoDModel::trajectory::ConnectionToVisualizer::runRequested, + this, &TwoDModelWidget::runButtonPressed); + connect(mConnToVisualizer, &twoDModel::trajectory::ConnectionToVisualizer::restartRequested, + this, &TwoDModelWidget::restartRequested); + connect(mUi->runButton, &QPushButton::clicked, mConnToVisualizer, + &twoDModel::trajectory::ConnectionToVisualizer::startPressed); + connect(mUi->stopButton, &QPushButton::clicked, mConnToVisualizer, + &twoDModel::trajectory::ConnectionToVisualizer::stopPressed); + setRunStopButtonsVisibility(); mUi->palette->unselect(); @@ -339,11 +355,12 @@ void TwoDModelWidget::connectUiButtons() connect(mRobotItemPopup, &RobotItemPopup::restoreRobotPositionClicked, this, &TwoDModelWidget::returnToStartMarker); connect(mRobotItemPopup, &RobotItemPopup::setRobotPositionClicked, this, &TwoDModelWidget::setStartMarker); connect(mUi->initialStateButton, &QAbstractButton::clicked, this, &TwoDModelWidget::returnToStartMarker); + connect(this, &TwoDModelWidget::restartRequested, this, &TwoDModelWidget::returnToStartMarker); connect(mUi->toggleDetailsButton, &QAbstractButton::clicked, this, &TwoDModelWidget::toggleDetailsVisibility); connect(mUi->trainingModeButton, &QAbstractButton::toggled, this, &TwoDModelWidget::trainingModeChanged); - mUi->trainingModeButton->setChecked(false); + mUi->trainingModeButton->setChecked(false); initRunStopButtons(); } @@ -401,6 +418,7 @@ void TwoDModelWidget::returnToStartMarker() ball->returnToStartPosition(); } saveWorldModelToRepo(); + mConnToVisualizer->restartPressed(); } void TwoDModelWidget::setStartMarker() @@ -718,6 +736,11 @@ Model &TwoDModelWidget::model() const return mModel; } +twoDModel::trajectory::ConnectionToVisualizer *TwoDModelWidget::connToVisualizer() +{ + return mConnToVisualizer; +} + void TwoDModelWidget::setController(ControllerInterface &controller) { mController = &controller; diff --git a/plugins/robots/common/twoDModel/twoDModel.pri b/plugins/robots/common/twoDModel/twoDModel.pri index c55550146c..49b4285716 100644 --- a/plugins/robots/common/twoDModel/twoDModel.pri +++ b/plugins/robots/common/twoDModel/twoDModel.pri @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -QT += widgets xml svg +QT += widgets xml svg network DEFINES += TWO_D_MODEL_LIBRARY @@ -65,6 +65,8 @@ HEADERS += \ $$PWD/include/twoDModel/robotModel/parts/lidar.h \ $$PWD/include/twoDModel/blocks/markerDownBlock.h \ $$PWD/include/twoDModel/blocks/markerUpBlock.h \ + $$PWD/src/engine/trajectory/connectionToVisualizer.h \ + $$PWD/src/engine/trajectory/trajectorySaver.h \ HEADERS += \ $$PWD/src/engine/twoDModelEngineApi.h \ @@ -130,6 +132,8 @@ HEADERS += \ SOURCES += \ $$PWD/include/twoDModel/robotModel/parts/colorSensorAmbient.cpp \ $$PWD/include/twoDModel/robotModel/parts/colorSensorReflected.cpp \ + $$PWD/src/engine/trajectory/connectionToVisualizer.cpp \ + $$PWD/src/engine/trajectory/trajectorySaver.cpp \ $$PWD/src/engine/twoDModelEngineFacade.cpp \ $$PWD/src/engine/twoDModelEngineApi.cpp \ $$PWD/src/engine/twoDModelGuiFacade.cpp \ @@ -222,3 +226,4 @@ SOURCES += \ FORMS += \ $$PWD/src/engine/view/twoDModelWidget.ui \ + diff --git a/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.cpp b/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.cpp index e0b0b7b0a7..effc852340 100644 --- a/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.cpp +++ b/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.cpp @@ -74,6 +74,8 @@ void TrikAdditionalPreferences::save() SettingsManager::setValue("TrikWebCameraRealName", mUi->cameraNameLineEdit->text()); SettingsManager::setValue("TRIK2DMailbox", mUi->mailboxCheckBox->isChecked()); SettingsManager::setValue("TRIK2DHullNumber", mUi->mailboxHullNumber->text()); + SettingsManager::setValue("UnityTcpServer", mUi->unityTcpServerLineEdit->currentText()); + SettingsManager::setValue("UnitySendData", mUi->unitySendDataCheckBox->isChecked()); mUi->robotImagePicker->save(); if (mailboxSavedState != mUi->mailboxCheckBox->isChecked()) { @@ -97,6 +99,10 @@ void TrikAdditionalPreferences::restoreSettings() mUi->simulatedCameraFrame->setVisible(not mUi->realCameraCheckBox->isChecked()); mUi->realCameraFrame->setVisible(mUi->realCameraCheckBox->isChecked()); mUi->mailboxCheckBox->setChecked(SettingsManager::value("TRIK2DMailbox").toBool()); + const auto &unityAddr = SettingsManager::value("UnityTcpServer").toString(); + mUi->unitySendDataCheckBox->setChecked(SettingsManager::value("UnitySendData").toBool()); + mUi->unityTcpServerLineEdit->insertItem(0, unityAddr); + mUi->unityTcpServerLineEdit->setCurrentText(unityAddr); mailboxSavedState = mUi->mailboxCheckBox->isChecked(); mUi->robotImagePicker->restore(); } diff --git a/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.ui b/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.ui index 787eec7e1a..287671e54c 100644 --- a/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.ui +++ b/plugins/robots/interpreters/trikKitInterpreterCommon/src/trikAdditionalPreferences.ui @@ -7,7 +7,7 @@ 0 0 357 - 427 + 521 @@ -79,13 +79,6 @@ - - - - Use real camera - - - @@ -131,6 +124,13 @@ + + + + Use real camera + + + @@ -147,14 +147,14 @@ - + Hull number: - + 999 @@ -164,15 +164,44 @@ + + + + Unity Connection Settings + + + + + + true + + + Enter Unity IP here + + + + + + + Send Data + + + + + + Qt::Vertical + + QSizePolicy::Fixed + 20 - 40 + 1 diff --git a/qrtranslations/fr/plugins/robots/trikKitInterpreterCommon_fr.ts b/qrtranslations/fr/plugins/robots/trikKitInterpreterCommon_fr.ts index eec1035d8c..111c780b45 100644 --- a/qrtranslations/fr/plugins/robots/trikKitInterpreterCommon_fr.ts +++ b/qrtranslations/fr/plugins/robots/trikKitInterpreterCommon_fr.ts @@ -37,12 +37,12 @@ - + Use real camera - + Network Settings @@ -51,8 +51,23 @@ Enable Mailbox + + + Unity Connection Settings + + + + + Enter Unity IP here + + + Send Data + + + + Hull number: @@ -67,7 +82,7 @@ - + Use images from project @@ -123,7 +138,7 @@ - + Information diff --git a/qrtranslations/fr/plugins/robots/twoDModel_fr.ts b/qrtranslations/fr/plugins/robots/twoDModel_fr.ts index c80bb123b6..32f35516a7 100644 --- a/qrtranslations/fr/plugins/robots/twoDModel_fr.ts +++ b/qrtranslations/fr/plugins/robots/twoDModel_fr.ts @@ -281,7 +281,7 @@ twoDModel::engine::TwoDModelEngineFacade - + Realistic physics' must be turned on to enjoy skittles and balls @@ -699,7 +699,7 @@ twoDModel::view::TwoDModelWidget - + Warning Attention @@ -717,7 +717,7 @@ Annuler - + Training mode: solution will not be checked @@ -749,7 +749,7 @@ - + Hide details diff --git a/qrtranslations/ru/plugins/robots/trikKitInterpreterCommon_ru.ts b/qrtranslations/ru/plugins/robots/trikKitInterpreterCommon_ru.ts index 390d042700..cf9aafd9af 100644 --- a/qrtranslations/ru/plugins/robots/trikKitInterpreterCommon_ru.ts +++ b/qrtranslations/ru/plugins/robots/trikKitInterpreterCommon_ru.ts @@ -41,17 +41,17 @@ Настройки камеры - + Use real camera Использовать камеру - + Camera: Камера: - + Use images from project Использовать запакованные в проект изображения @@ -66,7 +66,7 @@ Запаковывать изображения в проект - + Network Settings Настройки сети @@ -75,8 +75,23 @@ Enable Mailbox Активировать Mailbox + + + Unity Connection Settings + + + + + Enter Unity IP here + + + Send Data + + + + Hull number: Бортномер: @@ -86,7 +101,7 @@ - + Images: Изображения: @@ -179,7 +194,7 @@ Выберите папку - + Information Информация diff --git a/qrtranslations/ru/plugins/robots/twoDModel_ru.ts b/qrtranslations/ru/plugins/robots/twoDModel_ru.ts index 4df7869abc..f2ff072415 100644 --- a/qrtranslations/ru/plugins/robots/twoDModel_ru.ts +++ b/qrtranslations/ru/plugins/robots/twoDModel_ru.ts @@ -553,7 +553,7 @@ twoDModel::engine::TwoDModelEngineFacade - + Realistic physics' must be turned on to enjoy skittles and balls Реалистичная физика должна быть включена для взаимодействия с кеглями и мячами @@ -1042,7 +1042,7 @@ twoDModel::view::TwoDModelWidget - + Warning Предупреждение @@ -1060,7 +1060,7 @@ Отмена - + Training mode: solution will not be checked Режим тренировки: решение не будет проверяться @@ -1104,7 +1104,7 @@ Попытка загрузить слишком большое изображение может заморозить выполнение на некоторое время. Продолжить? - + Hide details Скрыть детали