From 2a076e91c6df7699284ab93e244894bb0f55336f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 10:46:14 -0800 Subject: [PATCH 01/36] =?UTF-8?q?=F0=9F=8E=88=204.14.0=20(#1261)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 21 ++++++++++++++++++--- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c10002405..5546ef954b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo4 VERSION 4.13.0) +project(ignition-gazebo4 VERSION 4.14.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 881e09fb96..7804bff869 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,20 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.14.0 (2021-12-20) + +1. Support battery draining start via topics + * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194) + * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250) + +1. Fix visualize lidar + * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224) + +1. Disable user commands light test on macOS + * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204) + ### Ignition Gazebo 4.13.0 (2021-11-15) 1. Prevent creation of spurious `` elements when saving worlds @@ -202,7 +217,7 @@ 1. Support configuring particle scatter ratio in particle emitter system. * [Pull Request 674](https://github.com/ignitionrobotics/ign-gazebo/pull/674) -1. Fix diffuse and ambient values for ackermann example. +1. Fix diffuse and ambient values for ackermann example. * [Pull Request 707](https://github.com/ignitionrobotics/ign-gazebo/pull/707) 1. Scenebroadcaster sensors. @@ -223,7 +238,7 @@ 1. Ackermann Steering Plugin. * [Pull Request 618](https://github.com/ignitionrobotics/ign-gazebo/pull/618) -1. Remove bounding box when model is deleted +1. Remove bounding box when model is deleted * [Pull Request 675](https://github.com/ignitionrobotics/ign-gazebo/pull/675) 1. Cache link poses to improve performance. @@ -232,7 +247,7 @@ 1. Check empty world name in Scene3d. * [Pull Request 662](https://github.com/ignitionrobotics/ign-gazebo/pull/662) -1. All changes up to 3.8.0. +1. All changes up to 3.8.0. ### Ignition Gazebo 4.6.0 (2021-03-01) From 6f58c919a95e8ab03864d25af022599e64bc090f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 18 Jan 2022 08:57:19 -0800 Subject: [PATCH 02/36] =?UTF-8?q?Prevent=20GzScene3D=20=F0=9F=92=A5=20if?= =?UTF-8?q?=20another=20scene=20is=20already=20loaded=20(#1294)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- src/gui/plugins/scene3d/GzScene3D.qml | 19 ++++-- src/gui/plugins/scene3d/Scene3D.cc | 87 +++++++++++++++++---------- src/gui/plugins/scene3d/Scene3D.hh | 44 +++++++++++++- 3 files changed, 112 insertions(+), 38 deletions(-) diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml index 4411fb5d7e..6dea55f64e 100644 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ b/src/gui/plugins/scene3d/GzScene3D.qml @@ -14,16 +14,18 @@ * limitations under the License. * */ +import IgnGazebo 1.0 as IgnGazebo +import QtGraphicalEffects 1.0 import QtQuick 2.9 import QtQuick.Controls 2.2 import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 import RenderWindow 1.0 -import QtGraphicalEffects 1.0 -import IgnGazebo 1.0 as IgnGazebo Rectangle { - width: 1000 - height: 800 + Layout.minimumWidth: 200 + Layout.minimumHeight: 200 + anchors.fill: parent /** * True to enable gamma correction @@ -38,6 +40,7 @@ Rectangle { anchors.fill: parent hoverEnabled: true acceptedButtons: Qt.NoButton + visible: GzScene3D.loadingError.length == 0 onEntered: { GzScene3D.OnFocusWindow() } @@ -50,6 +53,7 @@ Rectangle { id: renderWindow objectName: "renderWindow" anchors.fill: parent + visible: GzScene3D.loadingError.length == 0 /** * Message to be displayed over the render window @@ -120,4 +124,11 @@ Rectangle { standardButtons: Dialog.Ok } + Label { + anchors.fill: parent + anchors.margins: 10 + text: GzScene3D.loadingError + visible: (GzScene3D.loadingError.length > 0); + wrapMode: Text.WordWrap + } } diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 09ae8c5919..429294f7d4 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1643,17 +1643,25 @@ void IgnRenderer::HandleMouseViewControl() } ///////////////////////////////////////////////// -void IgnRenderer::Initialize() +std::string IgnRenderer::Initialize() { if (this->initialized) - return; + return std::string(); + + // Only one engine / scene / user camera is currently supported. + // Fail gracefully even before getting to renderUtil. + if (!rendering::loadedEngines().empty()) + { + return "Currently only one plugin providing a 3D scene is supported at a " + "time."; + } this->dataPtr->renderUtil.SetUseCurrentGLContext(true); this->dataPtr->renderUtil.Init(); rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); if (!scene) - return; + return "Failed to create a 3D scene."; auto root = scene->RootVisual(); @@ -1674,6 +1682,7 @@ void IgnRenderer::Initialize() this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); this->initialized = true; + return std::string(); } ///////////////////////////////////////////////// @@ -2066,6 +2075,12 @@ RenderThread::RenderThread() qRegisterMetaType(); } +///////////////////////////////////////////////// +void RenderThread::SetErrorCb(std::function _cb) +{ + this->errorCb = _cb; +} + ///////////////////////////////////////////////// void RenderThread::RenderNext() { @@ -2074,7 +2089,12 @@ void RenderThread::RenderNext() if (!this->ignRenderer.initialized) { // Initialize renderer - this->ignRenderer.Initialize(); + auto loadingError = this->ignRenderer.Initialize(); + if (!loadingError.empty()) + { + this->errorCb(QString::fromStdString(loadingError)); + return; + } } // check if engine has been successfully initialized @@ -2092,18 +2112,24 @@ void RenderThread::RenderNext() ///////////////////////////////////////////////// void RenderThread::ShutDown() { - this->context->makeCurrent(this->surface); + if (this->context && this->surface) + this->context->makeCurrent(this->surface); this->ignRenderer.Destroy(); - this->context->doneCurrent(); - delete this->context; + if (this->context) + { + this->context->doneCurrent(); + delete this->context; + } // schedule this to be deleted only after we're done cleaning up - this->surface->deleteLater(); + if (this->surface) + this->surface->deleteLater(); // Stop event processing, move the thread to GUI and make sure it is deleted. - this->moveToThread(QGuiApplication::instance()->thread()); + if (this->ignRenderer.initialized) + this->moveToThread(QGuiApplication::instance()->thread()); } @@ -2203,16 +2229,6 @@ void TextureNode::PrepareNode() RenderWindowItem::RenderWindowItem(QQuickItem *_parent) : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate) { - // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load - // this plugin twice, so shortcut here. Ideally this would be caught at - // Ignition Rendering. - static bool done{false}; - if (done) - { - return; - } - done = true; - this->setAcceptedMouseButtons(Qt::AllButtons); this->setFlag(ItemHasContents); this->dataPtr->renderThread = new RenderThread(); @@ -2378,18 +2394,6 @@ Scene3D::~Scene3D() = default; ///////////////////////////////////////////////// void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { - // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load - // this plugin twice, so shortcut here. Ideally this would be caught at - // Ignition Rendering. - static bool done{false}; - if (done) - { - ignerr << "Only one Scene3D is supported per process at the moment." - << std::endl; - return; - } - done = true; - auto renderWindow = this->PluginItem()->findChild(); if (!renderWindow) { @@ -2397,6 +2401,8 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) << "Render window will not be created" << std::endl; return; } + renderWindow->SetErrorCb(std::bind(&Scene3D::SetLoadingError, this, + std::placeholders::_1)); if (this->title.empty()) this->title = "3D Scene"; @@ -2891,6 +2897,19 @@ void Scene3D::OnFocusWindow() renderWindow->forceActiveFocus(); } +///////////////////////////////////////////////// +QString Scene3D::LoadingError() const +{ + return this->loadingError; +} + +///////////////////////////////////////////////// +void Scene3D::SetLoadingError(const QString &_loadingError) +{ + this->loadingError = _loadingError; + this->LoadingErrorChanged(); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetXYZSnap(const math::Vector3d &_xyz) { @@ -3180,6 +3199,12 @@ void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); } +///////////////////////////////////////////////// +void RenderWindowItem::SetErrorCb(std::function _cb) +{ + this->dataPtr->renderThread->SetErrorCb(_cb); +} + ///////////////////////////////////////////////// void RenderWindowItem::mousePressEvent(QMouseEvent *_e) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 1f743a4323..37b95f5db7 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -56,10 +56,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { class Scene3DPrivate; class RenderUtil; - /// \brief Creates a new ignition rendering scene or adds a user-camera to an - /// existing scene. It is possible to orbit the camera around the scene with + /// \brief Creates an ignition rendering scene and user camera. + /// It is possible to orbit the camera around the scene with /// the mouse. Use other plugins to manage objects in the scene. /// + /// Only one plugin displaying an Ignition Rendering scene can be used at a + /// time. + /// /// ## Configuration /// /// * \ : Optional render engine name, defaults to 'ogre'. @@ -88,6 +91,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { NOTIFY ErrorPopupTextChanged ) + /// \brief Loading error message + Q_PROPERTY( + QString loadingError + READ LoadingError + WRITE SetLoadingError + NOTIFY LoadingErrorChanged + ) + /// \brief Constructor public: Scene3D(); @@ -185,6 +196,20 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// the connection to work on the QML side signals: void popupError(); + /// \brief Get the loading error string. + /// \return String explaining the loading error. If empty, there's no error. + public: Q_INVOKABLE QString LoadingError() const; + + /// \brief Set the loading error message. + /// \param[in] _loadingError Error message. + public: Q_INVOKABLE void SetLoadingError(const QString &_loadingError); + + /// \brief Notify that loading error has changed + signals: void LoadingErrorChanged(); + + /// \brief Loading error message + public: QString loadingError; + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -210,7 +235,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void Render(); /// \brief Initialize the render engine - public: void Initialize(); + /// \return Error message if initialization failed. If empty, no errors + /// occurred. + public: std::string Initialize(); /// \brief Destroy camera associated with this renderer public: void Destroy(); @@ -519,6 +546,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _size Size of the texture signals: void TextureReady(int _id, const QSize &_size); + /// \brief Set a callback to be called in case there are errors. + /// \param[in] _cb Error callback + public: void SetErrorCb(std::function _cb); + + /// \brief Function to be called if there are errors. + public: std::function errorCb; + /// \brief Offscreen surface to render to public: QOffscreenSurface *surface = nullptr; @@ -726,6 +760,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _entity Scoped name of entity. public slots: void OnContextMenuRequested(QString _entity); + /// \brief Set a callback to be called in case there are errors. + /// \param[in] _cb Error callback + public: void SetErrorCb(std::function _cb); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; From 650b74658d0b8713de169b25a2e24c6d9c3e7ec3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 18 Jan 2022 08:59:02 -0800 Subject: [PATCH 03/36] Remove EachNew calls from sensor PreUpdates (#1281) Signed-off-by: Louise Poubel --- Migration.md | 4 ++ src/systems/air_pressure/AirPressure.cc | 46 ++++++++++++------ src/systems/altimeter/Altimeter.cc | 46 ++++++++++++------ src/systems/imu/Imu.cc | 52 ++++++++++++++------- src/systems/logical_camera/LogicalCamera.cc | 45 ++++++++++++------ src/systems/magnetometer/Magnetometer.cc | 47 +++++++++++++------ test/integration/air_pressure_system.cc | 6 ++- test/integration/altimeter_system.cc | 6 ++- test/integration/imu_system.cc | 6 ++- test/integration/logical_camera_system.cc | 30 +++++++----- test/integration/magnetometer_system.cc | 6 ++- 11 files changed, 211 insertions(+), 83 deletions(-) diff --git a/Migration.md b/Migration.md index 012dbafdb8..52e498a41b 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,10 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Gazebo 3.12.0 to 3.X.X + +* Some sensors will only have the `SensorTopic` component after the 1st iteration. + ## Ignition Gazebo 2.x to 3.x * Use ign-rendering3, ign-sensors3 and ign-gui3. diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index cff6ea8858..4346cf1f1b 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -49,30 +50,35 @@ using namespace systems; /// \brief Private AirPressure data class. class ignition::gazebo::systems::AirPressurePrivate { - /// \brief A map of air pressure entity to its vertical reference + /// \brief A map of air pressure entity to its sensor public: std::unordered_map> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _airPressure AirPressureSensor component. /// \param[in] _parent Parent entity component. public: void AddAirPressure( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent); /// \brief Create air pressure sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateAirPressureEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update air pressure sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -98,7 +104,21 @@ void AirPressure::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("AirPressure::PreUpdate"); - this->dataPtr->CreateAirPressureEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -116,6 +136,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + if (!_info.paused) { this->dataPtr->UpdateAirPressures(_ecm); @@ -134,7 +156,7 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AirPressurePrivate::AddAirPressure( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent) @@ -171,15 +193,13 @@ void AirPressurePrivate::AddAirPressure( math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) +void AirPressurePrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities"); if (!this->initialized) @@ -190,7 +210,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent)->bool { - AddAirPressure(_ecm, _entity, _airPressure, _parent); + this->AddAirPressure(_ecm, _entity, _airPressure, _parent); return true; }); this->initialized = true; @@ -203,7 +223,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent)->bool { - AddAirPressure(_ecm, _entity, _airPressure, _parent); + this->AddAirPressure(_ecm, _entity, _airPressure, _parent); return true; }); } diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index 4cc12f12bc..1b34fc63c1 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -51,30 +52,35 @@ using namespace systems; /// \brief Private Altimeter data class. class ignition::gazebo::systems::AltimeterPrivate { - /// \brief A map of altimeter entity to its vertical reference + /// \brief A map of altimeter entity to its sensor public: std::unordered_map> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _altimeter Altimeter component. /// \param[in] _parent Parent entity component. public: void AddAltimeter( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Altimeter *_altimeter, const components::ParentEntity *_parent); /// \brief Create altimeter sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateAltimeterEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update altimeter sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -99,7 +105,21 @@ void Altimeter::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Altimeter::PreUpdate"); - this->dataPtr->CreateAltimeterEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -116,6 +136,8 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -135,7 +157,7 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AltimeterPrivate::AddAltimeter( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Altimeter *_altimeter, const components::ParentEntity *_parent) @@ -173,15 +195,13 @@ void AltimeterPrivate::AddAltimeter( sensor->SetVerticalReference(verticalReference); sensor->SetPosition(verticalReference); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) +void AltimeterPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("Altimeter::CreateAltimeterEntities"); if (!this->initialized) @@ -192,7 +212,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) const components::Altimeter *_altimeter, const components::ParentEntity *_parent)->bool { - AddAltimeter(_ecm, _entity, _altimeter, _parent); + this->AddAltimeter(_ecm, _entity, _altimeter, _parent); return true; }); this->initialized = true; @@ -205,7 +225,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) const components::Altimeter *_altimeter, const components::ParentEntity *_parent)->bool { - AddAltimeter(_ecm, _entity, _altimeter, _parent); + this->AddAltimeter(_ecm, _entity, _altimeter, _parent); return true; }); } diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index d883f6cc91..a6a3ecf7d9 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -17,9 +17,10 @@ #include "Imu.hh" +#include #include +#include #include -#include #include @@ -56,6 +57,11 @@ class ignition::gazebo::systems::ImuPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + /// \brief Keep track of world ID, which is equivalent to the scene's /// root visual. /// Defaults to zero, which is considered invalid by Ignition Gazebo. @@ -64,21 +70,21 @@ class ignition::gazebo::systems::ImuPrivate /// True if the rendering component is initialized public: bool initialized = false; - /// \brief Create IMU sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateImuEntities(EntityComponentManager &_ecm); + /// \brief Create IMU sensors in ign-sensors + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update IMU sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. public: void Update(const EntityComponentManager &_ecm); /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _imu IMU component. /// \param[in] _parent Parent entity component. - public: void addIMU( - EntityComponentManager &_ecm, + public: void AddSensor( + const EntityComponentManager &_ecm, const Entity _entity, const components::Imu *_imu, const components::ParentEntity *_parent); @@ -102,7 +108,21 @@ void Imu::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Imu::PreUpdate"); - this->dataPtr->CreateImuEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -119,6 +139,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -137,8 +159,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void ImuPrivate::addIMU( - EntityComponentManager &_ecm, +void ImuPrivate::AddSensor( + const EntityComponentManager &_ecm, const Entity _entity, const components::Imu *_imu, const components::ParentEntity *_parent) @@ -186,15 +208,13 @@ void ImuPrivate::addIMU( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetOrientationReference(p.Rot()); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) +void ImuPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("ImuPrivate::CreateImuEntities"); // Get World Entity @@ -214,7 +234,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) const components::Imu *_imu, const components::ParentEntity *_parent)->bool { - addIMU(_ecm, _entity, _imu, _parent); + this->AddSensor(_ecm, _entity, _imu, _parent); return true; }); this->initialized = true; @@ -227,7 +247,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) const components::Imu *_imu, const components::ParentEntity *_parent)->bool { - addIMU(_ecm, _entity, _imu, _parent); + this->AddSensor(_ecm, _entity, _imu, _parent); return true; }); } diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 3b53fb4c0c..fbed671f66 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -59,23 +60,28 @@ class ignition::gazebo::systems::LogicalCameraPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _logicalCamera LogicalCamera component. /// \param[in] _parent Parent entity component. public: void AddLogicalCamera( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent); /// \brief Create logicalCamera sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateLogicalCameraEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update logicalCamera sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -101,7 +107,21 @@ void LogicalCamera::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCamera::PreUpdate"); - this->dataPtr->CreateLogicalCameraEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -118,6 +138,8 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -137,7 +159,7 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void LogicalCameraPrivate::AddLogicalCamera( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent) @@ -172,16 +194,13 @@ void LogicalCameraPrivate::AddLogicalCamera( math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void LogicalCameraPrivate::CreateLogicalCameraEntities( - EntityComponentManager &_ecm) +void LogicalCameraPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCameraPrivate::CreateLogicalCameraEntities"); if (!this->initialized) @@ -192,7 +211,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities( const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent)->bool { - AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; }); this->initialized = true; @@ -206,7 +225,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities( const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent)->bool { - AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; }); } diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index ade0e3dd08..bf974caa82 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -56,25 +57,30 @@ class ignition::gazebo::systems::MagnetometerPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _magnetometer Magnetometer component. /// \param[in] _worldField MagneticField component. /// \param[in] _parent Parent entity component. public: void AddMagnetometer( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Magnetometer *_magnetometer, const components::MagneticField *_worldField, const components::ParentEntity *_parent); /// \brief Create magnetometer sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateMagnetometerEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update magnetometer sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -100,7 +106,21 @@ void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Magnetometer::PreUpdate"); - this->dataPtr->CreateMagnetometerEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -117,6 +137,8 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -136,7 +158,7 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void MagnetometerPrivate::AddMagnetometer( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Magnetometer *_magnetometer, const components::MagneticField *_worldField, @@ -178,16 +200,13 @@ void MagnetometerPrivate::AddMagnetometer( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetWorldPose(p); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void MagnetometerPrivate::CreateMagnetometerEntities( - EntityComponentManager &_ecm) +void MagnetometerPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("MagnetometerPrivate::CreateMagnetometerEntities"); auto worldEntity = _ecm.EntityByComponents(components::World()); @@ -213,7 +232,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities( const components::Magnetometer *_magnetometer, const components::ParentEntity *_parent)->bool { - AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField, + _parent); return true; }); this->initialized = true; @@ -226,7 +246,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities( const components::Magnetometer *_magnetometer, const components::ParentEntity *_parent)->bool { - AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField, + _parent); return true; }); } diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 51e9000d90..52ad96ab50 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -62,7 +62,7 @@ TEST_F(AirPressureTest, AirPressure) // Create a system that checks sensor topic test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each( @@ -75,6 +75,10 @@ TEST_F(AirPressureTest, AirPressure) auto sensorComp = _ecm.Component(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index 4b391963ac..bac80b7c25 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -80,7 +80,7 @@ TEST_F(AltimeterTest, ModelFalling) test::Relay testSystem; std::vector poses; std::vector velocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 68147f4e30..c38c3cf6c7 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -89,7 +89,7 @@ TEST_F(ImuTest, ModelFalling) std::vector poses; std::vector accelerations; std::vector angularVelocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 32bae02ef5..4abcf00700 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -91,7 +91,7 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) // Create a system that checks sensor topics test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each( @@ -105,11 +105,15 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) auto sensorComp = _ecm.Component(_entity); EXPECT_NE(nullptr, sensorComp); - auto topicComp = - _ecm.Component(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) { - EXPECT_EQ(topic1, topicComp->Data()); + if (_info.iterations != 1) + { + auto topicComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic1, topicComp->Data()); + } } update1Checked = true; } @@ -119,11 +123,15 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) auto sensorComp = _ecm.Component(_entity); EXPECT_NE(nullptr, sensorComp); - auto topicComp = - _ecm.Component(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) { - EXPECT_EQ(topic2, topicComp->Data()); + if (_info.iterations != 1) + { + auto topicComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic2, topicComp->Data()); + } } update2Checked = true; } diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index cf29f976f8..e17be9b9f7 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -81,7 +81,7 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) From 749884db3f031f6159101c1d98880df6ac2deb46 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 21 Jan 2022 09:10:55 -0800 Subject: [PATCH 04/36] Buoyancy: fix center of volume's reference frame (#1302) Signed-off-by: Louise Poubel --- .../gazebo/components/CenterOfVolume.hh | 2 +- src/systems/buoyancy/Buoyancy.cc | 11 +- test/integration/buoyancy.cc | 76 +++++++++ test/worlds/center_of_volume.sdf | 160 ++++++++++++++++++ 4 files changed, 242 insertions(+), 7 deletions(-) create mode 100644 test/worlds/center_of_volume.sdf diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh index 4bd2389101..2096d2d5d0 100644 --- a/include/ignition/gazebo/components/CenterOfVolume.hh +++ b/include/ignition/gazebo/components/CenterOfVolume.hh @@ -33,7 +33,7 @@ namespace components /// \brief A component for an entity's center of volume. Units are in meters. /// The Vector3 value indicates center of volume of an entity. The /// position of the center of volume is relative to the pose of the parent - /// entity. + /// entity, which is usually a link. using CenterOfVolume = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume", CenterOfVolume) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 81a880a21a..0e7a286583 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -151,7 +151,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, _entity, components::Collision()); double volumeSum = 0; - ignition::math::Vector3d weightedPosSum = + ignition::math::Vector3d weightedPosInLinkSum = ignition::math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision @@ -210,16 +210,15 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } volumeSum += volume; - math::Pose3d pose = worldPose(collision, _ecm); - weightedPosSum += volume * pose.Pos(); + auto poseInLink = _ecm.Component(collision)->Data(); + weightedPosInLinkSum += volume * poseInLink.Pos(); } if (volumeSum > 0) { - // Store the center of volume - math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + // Store the center of volume expressed in the link frame _ecm.CreateComponent(_entity, components::CenterOfVolume( - weightedPosSum / volumeSum - linkWorldPose.Pos())); + weightedPosInLinkSum / volumeSum)); // Store the volume _ecm.CreateComponent(_entity, components::Volume(volumeSum)); diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 77445fdd61..7acfee7a5e 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -20,8 +20,10 @@ #include #include +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" #include "ignition/gazebo/components/CenterOfVolume.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" @@ -194,3 +196,77 @@ TEST_F(BuoyancyTest, Movement) server.Run(true, iterations, false); EXPECT_TRUE(finished); } + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, OffsetAndRotation) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "center_of_volume.sdf")); + + std::size_t iterations{0}; + fixture.OnPostUpdate([&]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get links + auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm); + ASSERT_EQ(1u, noOffsets.size()); + auto noOffset = *noOffsets.begin(); + EXPECT_NE(kNullEntity, noOffset); + + auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link", + _ecm); + ASSERT_EQ(1u, noOffsetRotateds.size()); + auto noOffsetRotated = *noOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, noOffsetRotated); + + auto withOffsets = entitiesFromScopedName("com_cov_offset::link", _ecm); + ASSERT_EQ(1u, withOffsets.size()); + auto withOffset = *withOffsets.begin(); + EXPECT_NE(kNullEntity, withOffset); + + auto withOffsetRotateds = entitiesFromScopedName( + "com_cov_offset_rotated::link", _ecm); + ASSERT_EQ(1u, withOffsetRotateds.size()); + auto withOffsetRotated = *withOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, withOffsetRotated); + + // Check CoVs have correct offsets + auto noOffsetCoV = _ecm.Component(noOffset); + ASSERT_NE(noOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data()); + + auto noOffsetRotatedCoV = _ecm.Component( + noOffsetRotated); + ASSERT_NE(noOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data()); + + auto withOffsetCoV = _ecm.Component(withOffset); + ASSERT_NE(withOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data()); + + auto withOffsetRotatedCoV = _ecm.Component( + withOffsetRotated); + ASSERT_NE(withOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetRotatedCoV->Data()); + + // Check that all objects are neutrally buoyant and stay still + auto noOffsetPose = worldPose(noOffset, _ecm); + EXPECT_EQ(math::Pose3d(), noOffsetPose); + + auto noOffsetRotatedPose = worldPose(noOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 0, 0, 0.1, 0.2, 0.3), noOffsetRotatedPose); + + auto withOffsetPose = worldPose(withOffset, _ecm); + EXPECT_EQ(math::Pose3d(0, 3, 0, 0, 0, 0), withOffsetPose); + + auto withOffsetRotatedPose = worldPose(withOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 3, 0, 0.1, 0.2, 0.3), withOffsetRotatedPose); + + iterations++; + }).Finalize(); + + std::size_t targetIterations{1000}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); +} diff --git a/test/worlds/center_of_volume.sdf b/test/worlds/center_of_volume.sdf new file mode 100644 index 0000000000..0972ca554d --- /dev/null +++ b/test/worlds/center_of_volume.sdf @@ -0,0 +1,160 @@ + + + + + + 0 + + + + + 1000 + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + -3 0 0 0.1 0.2 0.3 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + 0 3 0 0 0 0 + + 0 0 0 0 0 0 + + 1 1 1 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + -3 3 0 0.1 0.2 0.3 + + 0 0 0 0 0 0 + + 1 1 1 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + + From 912e2cebec89a96fd8632ed5358d6036f7873ef6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 25 Jan 2022 11:36:36 -0800 Subject: [PATCH 05/36] Update source install instructions (#1311) Signed-off-by: Louise Poubel --- tutorials/install.md | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/tutorials/install.md b/tutorials/install.md index 97d4e598ac..3260f10154 100644 --- a/tutorials/install.md +++ b/tutorials/install.md @@ -59,33 +59,36 @@ feature which hasn't been released yet. ### Ubuntu 18.04 or above -1. Enable the Ignition software repositories: +1. Install tools + ``` + sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget + ``` + +2. Enable the Ignition software repositories: ``` sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - sudo apt-get update ``` -2. Install package dependencies: +3. Clone repository ``` git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#> - export SYSTEM_VERSION=bionic - sudo apt -y install \ - $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/ignition\|sdf/d' | tr '\n' ' ') ``` -3. (Ubuntu 18.04 only) Configure gcc8 +4. Install package dependencies (including other Ignition libraries): ``` - sudo apt-get install g++-8 - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 + sudo apt -y install \ + $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | tr '\n' ' ')) ``` -4. Clone the repository if you haven't already. +5. (Ubuntu 18.04 only) Configure gcc8 ``` - git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#> + sudo apt-get install g++-8 + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 ``` -5. Configure and build. +6. Configure and build. ``` cd ign-gazebo mkdir build From ebb3a9ad8a7484eeb97bf2585f7731e7d4225f9b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 28 Jan 2022 16:48:17 +0800 Subject: [PATCH 06/36] Log an error if JointPositionController cannot find the joint. (citadel retarget) (#1314) In the event a user enters the wrong name for a certain joint, the JointPositionController system will silently fail. This PR adds a simple error message that tells the user that the joint was not found. Signed-off-by: Arjo Chakravarty --- .../joint_position_controller/JointPositionController.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 067c622161..9530256eff 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -192,8 +192,17 @@ void JointPositionController::PreUpdate( this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); } + // If the joint is still not found then warn the user, they may have entered + // the wrong joint name. if (this->dataPtr->jointEntity == kNullEntity) + { + static bool warned = false; + if(!warned) + ignerr << "Could not find joint with name [" + << this->dataPtr->jointName <<"]\n"; + warned = true; return; + } // Nothing left to do if paused. if (_info.paused) From 717a7e91b73e968d836689d23dcdbf2579e7fbce Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 3 Feb 2022 17:40:22 -0800 Subject: [PATCH 07/36] Load and run visual plugin (system) on GUI side (#1275) * load and run visual plugins on gui end Signed-off-by: Ian Chen * scene update event emitted on both server and gui side Signed-off-by: Ian Chen * shader param update working Signed-off-by: Ian Chen * sim time, constants, full example working Signed-off-by: Ian Chen * add integration test Signed-off-by: Ian Chen * code cleanup Signed-off-by: Ian Chen * more code cleanup Signed-off-by: Ian Chen * style fixes and add some comments Signed-off-by: Ian Chen * review changes Signed-off-by: Ian Chen * require display for shader param test Signed-off-by: Ian Chen * style and comment Signed-off-by: Ian Chen --- examples/worlds/shader_param.sdf | 268 ++++++++++++ include/ignition/gazebo/components/Visual.hh | 54 +++ include/ignition/gazebo/gui/GuiEvents.hh | 24 ++ include/ignition/gazebo/rendering/Events.hh | 12 + .../ignition/gazebo/rendering/RenderUtil.hh | 4 + src/SdfEntityCreator.cc | 11 + src/SimulationRunner.hh | 3 - src/SimulationRunner_TEST.cc | 21 + src/gui/GuiEvents.cc | 30 ++ src/gui/GuiRunner.cc | 144 ++++++- src/gui/GuiRunner.hh | 10 + .../plugins/scene_manager/GzSceneManager.cc | 55 +++ src/rendering/RenderUtil.cc | 13 + src/systems/CMakeLists.txt | 1 + src/systems/sensors/Sensors.cc | 4 +- src/systems/shader_param/CMakeLists.txt | 8 + src/systems/shader_param/ShaderParam.cc | 396 ++++++++++++++++++ src/systems/shader_param/ShaderParam.hh | 100 +++++ test/integration/CMakeLists.txt | 1 + test/integration/shader_param_system.cc | 110 +++++ test/plugins/CMakeLists.txt | 1 + test/plugins/TestVisualSystem.cc | 23 + test/plugins/TestVisualSystem.hh | 81 ++++ test/worlds/plugins.sdf | 7 + 24 files changed, 1371 insertions(+), 10 deletions(-) create mode 100644 examples/worlds/shader_param.sdf create mode 100644 src/systems/shader_param/CMakeLists.txt create mode 100644 src/systems/shader_param/ShaderParam.cc create mode 100644 src/systems/shader_param/ShaderParam.hh create mode 100644 test/integration/shader_param_system.cc create mode 100644 test/plugins/TestVisualSystem.cc create mode 100644 test/plugins/TestVisualSystem.hh diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf new file mode 100644 index 0000000000..db3d48ca78 --- /dev/null +++ b/examples/worlds/shader_param.sdf @@ -0,0 +1,268 @@ + + + + + + + + + ogre2 + 0.8 0.8 0.8 + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + docked + + + + + + + docked + + + + + + RGB camera + floating + 350 + 315 + + camera + false + + + + Depth camera + floating + 350 + 315 + 500 + + depth_camera + false + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.6 0.6 0.6 1 + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.0 0.5 0 0 0 + deformable_sphere + 3 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere + + + + true + 2.5 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + camera + + + 10 + depth_camera + + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + + + diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index 9d90871a7b..de94818059 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -17,6 +17,11 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ #define IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ +#include + +#include +#include + #include #include #include @@ -27,11 +32,60 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + class SdfElementSerializer + { + /// \brief Serialization for `sdf::Model`. + /// \param[in] _out Output stream. + /// \param[in] _elem Visual plugin elem to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const sdf::ElementPtr &_elem) + { + _out << "" + << "" + << _elem->ToString("") + << ""; + return _out; + } + + /// \brief Deserialization for `sdf::Element`. + /// \param[in] _in Input stream. + /// \param[out] _elem Visual plugin elem to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + sdf::ElementPtr &_elem) + { + std::string sdfStr(std::istreambuf_iterator(_in), {}); + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + bool result = sdf::readString(sdfStr, sdfParsed); + if (!result) + { + ignerr << "Unable to deserialize sdf::ElementPtr" << std::endl; + return _in; + } + + _elem = sdfParsed->Root()->GetFirstElement(); + return _in; + } + }; +} + namespace components { /// \brief A component that identifies an entity as being a visual. using Visual = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) + + /// \brief A component that contains a visual plugin's SDF element. + using VisualPlugin = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", + VisualPlugin) } } } diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 4407d68598..afbb751879 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -25,8 +25,11 @@ #include #include #include + #include #include +#include + #include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -211,6 +214,27 @@ namespace events IGN_UTILS_IMPL_PTR(dataPtr) }; + /// \brief Event that notifies a visual plugin is to be loaded + class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent + { + /// \brief Constructor + /// \param[in] _entity Visual entity id + /// \param[in] _elem Visual plugin SDF element + public: explicit VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem); + + /// \brief Get the entity to load the visual plugin for + public: ignition::gazebo::Entity Entity() const; + + /// \brief Get the sdf element of the visual plugin + public: sdf::ElementPtr Element() const; + + static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; } // namespace events } } // namespace gui diff --git a/include/ignition/gazebo/rendering/Events.hh b/include/ignition/gazebo/rendering/Events.hh index d17b3922c5..683c66f529 100644 --- a/include/ignition/gazebo/rendering/Events.hh +++ b/include/ignition/gazebo/rendering/Events.hh @@ -32,6 +32,18 @@ namespace ignition /// more information about events. namespace events { + /// \brief The render event is emitted when the the scene manager is + /// updated with contents from the ECM. This event is emitted + /// before the PreRender event on the server side in the rendering + /// thread. It is also accessible on the GUI side. + /// + /// For example: + /// \code + /// eventManager.Emit(); + /// \endcode + using SceneUpdate = ignition::common::EventT; + /// \brief The render event is emitted before rendering updates. /// The event is emitted in the rendering thread so rendering /// calls can ben make in this event callback diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 6110b2083c..a757ec5378 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -214,6 +214,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _active True if active. public: void SetTransformActive(bool _active); + /// \brief Set the event manager to use + /// \param[in] _mgr Event manager to set to. + public: void SetEventManager(EventManager *_mgr); + /// \brief Private data pointer. private: std::unique_ptr dataPtr; }; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index ae98f41ec7..d54e1f377f 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -749,6 +749,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) components::Material(*_visual->Material())); } + // store the plugin in a component + if (_visual->Element()) + { + sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); + if (pluginElem) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::VisualPlugin(pluginElem)); + } + } + // Keep track of visuals so we can load their plugins after loading the // entire model and having its full scoped name. this->dataPtr->newVisuals[visualEntity] = _visual->Element(); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index a52210425e..5da194e209 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -495,9 +495,6 @@ namespace ignition /// \brief Mutex to protect pendingSystems private: mutable std::mutex pendingSystemsMutex; - /// \brief Systems implementing Configure - private: std::vector systemsConfigure; - /// \brief Systems implementing PreUpdate private: std::vector systemsPreupdate; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index cc9b33161a..17641a873c 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1234,6 +1234,18 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) }); EXPECT_NE(kNullEntity, sensorId); + // Get visual entity + Entity visualId{kNullEntity}; + runner.EntityCompMgr().Each([&]( + const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Visual *_visual)->bool + { + EXPECT_NE(nullptr, _visual); + visualId = _entity; + return true; + }); + EXPECT_NE(kNullEntity, visualId); + // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; auto worldComponentId = ignition::common::hash64(worldComponentName); @@ -1258,6 +1270,14 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, sensorComponentId)); + // Check component registered by visual plugin + std::string visualComponentName{"VisualPluginComponent"}; + auto visualComponentId = ignition::common::hash64(visualComponentName); + + EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(visualComponentId)); + EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(visualId, + visualComponentId)); + // Clang re-registers components between tests. If we don't unregister them // beforehand, the new plugin tries to create a storage type from a previous // plugin, causing a crash. @@ -1268,6 +1288,7 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) components::Factory::Instance()->Unregister(worldComponentId); components::Factory::Instance()->Unregister(modelComponentId); components::Factory::Instance()->Unregister(sensorComponentId); + components::Factory::Instance()->Unregister(visualComponentId); #endif } diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index ed195212ed..a89a5d8254 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -50,6 +50,15 @@ class ignition::gazebo::gui::events::ModelEditorAddEntity::Implementation public: ignition::gazebo::Entity parent; }; +class ignition::gazebo::gui::events::VisualPlugin::Implementation +{ + /// \brief Entity to load the visual plugin for + public: ignition::gazebo::Entity entity; + + /// \brief Sdf element of the visual plugin + public: sdf::ElementPtr element; +}; + using namespace ignition; using namespace gazebo; using namespace gui; @@ -132,3 +141,24 @@ QMap &ModelEditorAddEntity::Data() { return this->dataPtr->data; } + +///////////////////////////////////////////////// +VisualPlugin::VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem) : + QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->entity = _entity; + this->dataPtr->element = _elem; +} + +///////////////////////////////////////////////// +ignition::gazebo::Entity VisualPlugin::Entity() const +{ + return this->dataPtr->entity; +} + +///////////////////////////////////////////////// +sdf::ElementPtr VisualPlugin::Element() const +{ + return this->dataPtr->element; +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 50a2c3f075..2a70c033ac 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -15,6 +15,10 @@ * */ +#include +#include +#include + #include #include #include @@ -28,7 +32,9 @@ #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include #include "ignition/gazebo/gui/GuiSystem.hh" +#include "ignition/gazebo/SystemLoader.hh" #include "GuiRunner.hh" @@ -68,6 +74,31 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Name of WorldControl service public: std::string controlService; + + /// \brief System loader for loading ign-gazebo systems + public: std::unique_ptr systemLoader; + + /// \brief Mutex to protect systemLoader + public: std::mutex systemLoadMutex; + + /// \brief Events containing visual plugins to load + public: std::vector> + visualPlugins; + + /// \brief Systems implementing PreUpdate + public: std::vector systems; + + /// \brief Systems implementing PreUpdate + public: std::vector systemsPreupdate; + + /// \brief Systems implementing Update + public: std::vector systemsUpdate; + + /// \brief Systems implementing PostUpdate + public: std::vector systemsPostupdate; + + /// \brief Manager of all events. + public: EventManager eventMgr; }; ///////////////////////////////////////////////// @@ -92,7 +123,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) // so that an offset is not required this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I32 / 2); - auto win = gui::App()->findChild(); + auto win = ignition::gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); winWorldNames.append(QString::fromStdString(_worldName)); win->setProperty("worldNames", winWorldNames); @@ -136,7 +167,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == ignition::gui::events::WorldControl::kType) { auto worldControlEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (worldControlEvent) { msgs::WorldControlState req; @@ -162,6 +193,20 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); } } + else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType) + { + auto visualPluginEvent = + reinterpret_cast(_event); + if (visualPluginEvent) + { + std::lock_guard lock(this->dataPtr->systemLoadMutex); + + Entity entity = visualPluginEvent->Entity(); + sdf::ElementPtr pluginElem = visualPluginEvent->Element(); + this->dataPtr->visualPlugins.push_back( + std::make_pair(entity, pluginElem)); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -171,7 +216,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) void GuiRunner::RequestState() { // set up service for async state response callback - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv); @@ -225,7 +270,7 @@ void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) // todo(anyone) store reqSrv string in a member variable and use it here // and in RequestState() - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; this->dataPtr->node.UnadvertiseSrv(reqSrv); @@ -264,7 +309,8 @@ void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) ///////////////////////////////////////////////// void GuiRunner::UpdatePlugins() { - auto plugins = gui::App()->findChildren(); + // gui plugins + auto plugins = ignition::gui::App()->findChildren(); for (auto plugin : plugins) { plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); @@ -272,4 +318,92 @@ void GuiRunner::UpdatePlugins() this->dataPtr->ecm.ClearRemovedComponents(); this->dataPtr->ecm.ClearNewlyCreatedEntities(); this->dataPtr->ecm.ProcessRemoveEntityRequests(); + + // ign-gazebo systems + this->LoadSystems(); + this->UpdateSystems(); +} + +///////////////////////////////////////////////// +void GuiRunner::LoadSystems() +{ + std::lock_guard lock(this->dataPtr->systemLoadMutex); + // currently only support systems that are visual plugins + for (auto &visualPlugin : this->dataPtr->visualPlugins) + { + Entity entity = visualPlugin.first; + sdf::ElementPtr pluginElem = visualPlugin.second; + auto filename = pluginElem->Get("filename"); + auto name = pluginElem->Get("name"); + if (filename != "__default__" && name != "__default__") + { + std::optional system; + if (!this->dataPtr->systemLoader) + this->dataPtr->systemLoader = std::make_unique(); + system = this->dataPtr->systemLoader->LoadPlugin( + filename, name, pluginElem); + if (system) + { + SystemPluginPtr sys = system.value(); + this->dataPtr->systems.push_back(sys); + this->dataPtr->systemsPreupdate.push_back( + sys->QueryInterface()); + this->dataPtr->systemsUpdate.push_back( + sys->QueryInterface()); + this->dataPtr->systemsPostupdate.push_back( + sys->QueryInterface()); + + auto sysConfigure = sys->QueryInterface(); + if (sysConfigure) + { + sysConfigure->Configure(entity, pluginElem, this->dataPtr->ecm, + this->dataPtr->eventMgr); + } + igndbg << "Loaded system [" << name + << "] for entity [" << entity << "] in GUI" + << std::endl; + } + } + } + this->dataPtr->visualPlugins.clear(); +} + +///////////////////////////////////////////////// +void GuiRunner::UpdateSystems() +{ + IGN_PROFILE("GuiRunner::UpdateSystems"); + + { + IGN_PROFILE("PreUpdate"); + for (auto& system : this->dataPtr->systemsPreupdate) + { + if (system) + system->PreUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("Update"); + for (auto& system : this->dataPtr->systemsUpdate) + { + if (system) + system->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("PostUpdate"); + // \todo(anyone) Do PostUpdates in parallel + for (auto& system : this->dataPtr->systemsPostupdate) + { + if (system) + system->PostUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } +} + +///////////////////////////////////////////////// +EventManager &GuiRunner::GuiEventManager() const +{ + return this->dataPtr->eventMgr; } diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index 1a06098161..bc5a730553 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -25,6 +25,7 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -46,6 +47,9 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Destructor public: ~GuiRunner() override; + /// \brief Get the event manager for the gui + public: EventManager &GuiEventManager() const; + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; @@ -74,6 +78,12 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 private: Q_INVOKABLE void UpdatePlugins(); + /// \brief Load systems + private: void LoadSystems(); + + /// \brief Update systems + private: void UpdateSystems(); + /// \brief Pointer to private data. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index cccb6caaa4..48c0735f10 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -14,10 +14,15 @@ * limitations under the License. * */ + +#include #include +#include "../../GuiRunner.hh" #include "GzSceneManager.hh" +#include + #include #include #include @@ -28,6 +33,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" @@ -58,6 +64,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Mutex to protect gui event and system upate call race conditions /// for newEntities and removedEntities public: std::mutex newRemovedEntityMutex; + + /// \brief Indicates whether initial visual plugins have been loaded or not. + public: bool initializedVisualPlugins = false; }; } } @@ -102,6 +111,42 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + // load visual plugin on gui side + std::map pluginElems; + if (!this->dataPtr->initializedVisualPlugins) + { + _ecm.Each( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + this->dataPtr->initializedVisualPlugins = true; + } + else + { + _ecm.EachNew( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + } + for (const auto &it : pluginElems) + { + ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + it.first, it.second); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &visualPluginEvent); + } + // Emit entities created / removed event for gui::Plugins which don't have // direct access to the ECM. std::set created; @@ -163,6 +208,16 @@ void GzSceneManagerPrivate::OnRender() return; this->renderUtil.SetScene(this->scene); + + auto runners = ignition::gui::App()->findChildren(); + if (runners.empty() || runners[0] == nullptr) + { + ignerr << "Internal error: no GuiRunner found." << std::endl; + } + else + { + this->renderUtil.SetEventManager(&runners[0]->GuiEventManager()); + } } this->renderUtil.Update(); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 3df544c588..0679db3769 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -89,6 +89,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/rendering/Events.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/SceneManager.hh" #include "ignition/gazebo/rendering/MarkerManager.hh" @@ -193,6 +194,9 @@ class ignition::gazebo::RenderUtilPrivate const components::Name *_name, const components::ParentEntity *_parent); + /// \brief Event manager used for emitting render / scene events + public: EventManager *eventManager{nullptr}; + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1583,6 +1587,9 @@ void RenderUtil::Update() } } } + + if (this->dataPtr->eventManager) + this->dataPtr->eventManager->Emit(); } ////////////////////////////////////////////////// @@ -3637,3 +3644,9 @@ void RenderUtilPrivate::CreateLight( this->newLights.push_back(std::make_tuple(_entity, _light->Data(), _name->Data(), _parent->Data())); } + +////////////////////////////////////////////////// +void RenderUtil::SetEventManager(EventManager *_mgr) +{ + this->dataPtr->eventManager = _mgr; +} diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 17514029f8..5054dad438 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -128,6 +128,7 @@ add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) +add_subdirectory(shader_param) add_subdirectory(thermal) add_subdirectory(thruster) add_subdirectory(touch_plugin) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 5d802189b1..d5309a82c7 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -242,7 +242,6 @@ void SensorsPrivate::RunOnce() this->renderUtil.Update(); } - if (!this->activeSensors.empty()) { this->sensorMaskMutex.lock(); @@ -422,8 +421,9 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->renderUtil.SetEnableSensors(true, std::bind(&Sensors::CreateSensor, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - this->dataPtr->renderUtil.SetRemoveSensorCb( + this->dataPtr->renderUtil.SetRemoveSensorCb( std::bind(&Sensors::RemoveSensor, this, std::placeholders::_1)); + this->dataPtr->renderUtil.SetEventManager(&_eventMgr); // parse sensor-specific data auto worldEntity = _ecm.EntityByComponents(components::World()); diff --git a/src/systems/shader_param/CMakeLists.txt b/src/systems/shader_param/CMakeLists.txt new file mode 100644 index 0000000000..0d14af33f3 --- /dev/null +++ b/src/systems/shader_param/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(shader-param + SOURCES + ShaderParam.cc + PUBLIC_LINK_LIBS + ${rendering_target} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc new file mode 100644 index 0000000000..feab7709b2 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.cc @@ -0,0 +1,396 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 "ShaderParam.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::ShaderParamPrivate +{ + /// \brief Data structure for storing shader param info + public: class ShaderParamValue + { + /// \brief shader type: vertex or fragment + public: std::string shader; + + /// \brief variable type: int, float, float_array, int_array + /// \todo(anyone) support samplers + public: std::string type; + + /// \brief variable name of param + public: std::string name; + + /// \brief param value + public: std::string value; + }; + + /// \brief Path to vertex shader + public: std::string vertexShaderUri; + + /// \brief Path to fragment shader + public: std::string fragmentShaderUri; + + /// \brief Mutex to protect sim time updates. + public: std::mutex mutex; + + /// \brief Connection to pre-render event callback + public: ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Name of visual this plugin is attached to + public: std::string visualName; + + /// \brief Pointer to visual + public: rendering::VisualPtr visual; + + /// \brief Material used by this visual + public: rendering::MaterialPtr material; + + /// \brief Pointer to scene + public: rendering::ScenePtr scene; + + /// \brief Entity id of the visual + public: Entity entity = kNullEntity; + + /// \brief A list of shader params + public: std::vector shaderParams; + + /// \brief Time params that will be updated every iteration + public: std::vector timeParams; + + /// \brief Current sim time + public: std::chrono::steady_clock::duration currentSimTime; + + /// \brief All rendering operations must happen within this call + public: void OnUpdate(); +}; + +///////////////////////////////////////////////// +ShaderParam::ShaderParam() + : System(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ShaderParam::~ShaderParam() +{ +} + +///////////////////////////////////////////////// +void ShaderParam::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + IGN_PROFILE("ShaderParam::Configure"); + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto sdf = const_cast(_sdf.get()); + + if (sdf->HasElement("param")) + { + // loop and parse all shader params + sdf::ElementPtr paramElem = sdf->GetElement("param"); + while (paramElem) + { + if (!paramElem->HasElement("shader") || + !paramElem->HasElement("name")) + { + ignerr << " must have and sdf elements" + << std::endl; + paramElem = paramElem->GetNextElement("param"); + continue; + } + std::string shaderType = paramElem->Get("shader"); + std::string paramName = paramElem->Get("name"); + + std::string type = paramElem->Get("type", "float").first; + std::string value = paramElem->Get("value", "").first; + + ShaderParamPrivate::ShaderParamValue spv; + spv.shader = shaderType; + spv.name = paramName; + spv.value = value; + spv.type = type; + this->dataPtr->shaderParams.push_back(spv); + paramElem = paramElem->GetNextElement("param"); + } + } + + // parse path to shaders + if (sdf->HasElement("shader")) + { + sdf::ElementPtr shaderElem = sdf->GetElement("shader"); + if (!shaderElem->HasElement("vertex") || + !shaderElem->HasElement("fragment")) + { + ignerr << " must have and sdf elements" + << std::endl; + } + else + { + auto modelEntity = topLevelModel(_entity, _ecm); + auto modelPath = + _ecm.ComponentData(modelEntity); + sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex"); + this->dataPtr->vertexShaderUri = common::findFile( + asFullPath(vertexElem->Get(), modelPath.value())); + sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment"); + this->dataPtr->fragmentShaderUri = common::findFile( + asFullPath(fragmentElem->Get(), modelPath.value())); + } + } + + this->dataPtr->entity = _entity; + auto nameComp = _ecm.Component(_entity); + this->dataPtr->visualName = nameComp->Data(); + + // connect to the SceneUpdate event + // the callback is executed in the rendering thread so do all + // rendering operations in that thread + this->dataPtr->connection = + _eventMgr.Connect( + std::bind(&ShaderParamPrivate::OnUpdate, this->dataPtr.get())); +} + +////////////////////////////////////////////////// +void ShaderParam::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &) +{ + IGN_PROFILE("ShaderParam::PreUpdate"); + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->currentSimTime = _info.simTime; +} + +////////////////////////////////////////////////// +void ShaderParamPrivate::OnUpdate() +{ + std::lock_guard lock(this->mutex); + if (this->visualName.empty()) + return; + + if (!this->scene) + this->scene = rendering::sceneFromFirstRenderEngine(); + + if (!this->scene) + return; + + if (!this->visual) + { + // this does a breadth first search for visual with the entity id + // \todo(anyone) provide a helper function in RenderUtil to search for + // visual by entity id? + auto rootVis = scene->RootVisual(); + std::list nodes; + nodes.push_back(rootVis); + while (!nodes.empty()) + { + auto n = nodes.front(); + nodes.pop_front(); + if (n && n->HasUserData("gazebo-entity")) + { + // RenderUti stores gazebo-entity user data as int + // \todo(anyone) Change this to uint64_t in Ignition H? + auto variant = n->UserData("gazebo-entity"); + const int *value = std::get_if(&variant); + if (value && *value == static_cast(this->entity)) + { + this->visual = std::dynamic_pointer_cast(n); + break; + } + } + for (unsigned int i = 0; i < n->ChildCount(); ++i) + nodes.push_back(n->ChildByIndex(i)); + } + } + + if (!this->visual) + return; + + // get the material and set shaders + if (!this->material) + { + auto mat = scene->CreateMaterial(); + mat->SetVertexShader(this->vertexShaderUri); + mat->SetFragmentShader(this->fragmentShaderUri); + this->visual->SetMaterial(mat); + scene->DestroyMaterial(mat); + this->material = this->visual->Material(); + } + + if (!this->material) + return; + + // set the shader params read from SDF + // this is only done once + for (const auto & spv : this->shaderParams) + { + std::vector values = common::split(spv.value, " "); + + int intValue = 0; + float floatValue = 0; + std::vector floatArrayValue; + + rendering::ShaderParam::ParamType paramType = + rendering::ShaderParam::PARAM_NONE; + + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + { + params = this->material->FragmentShaderParams(); + } + else if (spv.shader == "vertex") + { + params = this->material->VertexShaderParams(); + } + + // if no is specified, this could be a constant + if (values.empty()) + { + // \todo handle args for constants + (*params)[spv.name] = intValue; + } + // float / int + else if (values.size() == 1u) + { + std::string str = values[0]; + + // TIME is reserved keyword for sim time + if (str == "TIME") + { + this->timeParams.push_back(spv); + continue; + } + + // if is not empty, respect the specified type + if (!spv.type.empty()) + { + if (spv.type == "int") + { + intValue = std::stoi(str); + paramType = rendering::ShaderParam::PARAM_INT; + } + else if (spv.type == "float") + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + else + { + // \todo(anyone) support texture samplers + } + } + // else do our best guess at what the type is + else + { + std::string::size_type sz; + int n = std::stoi(str, &sz); + if ( sz == str.size()) + { + intValue = n; + paramType = rendering::ShaderParam::PARAM_INT; + } + else + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + } + } + // arrays + else + { + // int array + if (!spv.type.empty() && spv.type == "int_array") + { + for (const auto &v : values) + floatArrayValue.push_back(std::stoi(v)); + paramType = rendering::ShaderParam::PARAM_INT_BUFFER; + } + // treat everything else as float_array + else + { + for (const auto &v : values) + floatArrayValue.push_back(std::stof(v)); + paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER; + } + } + + // set the params + if (paramType == rendering::ShaderParam::PARAM_INT) + { + (*params)[spv.name] = intValue; + } + else if (paramType == rendering::ShaderParam::PARAM_FLOAT) + { + (*params)[spv.name] = floatValue; + } + else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER || + paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER) + { + (*params)[spv.name].InitializeBuffer(floatArrayValue.size()); + float *fv = &floatArrayValue[0]; + (*params)[spv.name].UpdateBuffer(fv); + } + } + this->shaderParams.clear(); + + // time variables need to be updated every iteration + for (const auto & spv : this->timeParams) + { + float floatValue = (std::chrono::duration_cast( + this->currentSimTime).count()) * 1e-9; + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + params = this->material->FragmentShaderParams(); + else if (spv.shader == "vertex") + params = this->material->VertexShaderParams(); + (*params)[spv.name] = floatValue; + } +} + +IGNITION_ADD_PLUGIN(ShaderParam, + ignition::gazebo::System, + ShaderParam::ISystemConfigure, + ShaderParam::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ShaderParam, + "ignition::gazebo::systems::ShaderParam") diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh new file mode 100644 index 0000000000..d77e8bfd78 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.hh @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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. + * +*/ + +#ifndef IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_ +#define IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_ + +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class ShaderParamPrivate; + + /// \brief A plugin for setting shaders to a visual and its params + /// + /// Plugin parameters: + /// + /// + /// Path to vertex program + /// Path to fragment program + /// Shader parameter - can be repeated within plugin SDF element + /// Name of uniform variable bound to the shader + /// Type of shader, i.e. vertex, fragment + /// Variable type: float, int, float_array, int_array + /// Value to set the shader parameter to. The vallue string can + /// be an int, float, or a space delimited array of ints or + /// floats. It can also be 'TIME', in which case the value will + /// be bound to sim time. + /// + /// Example usage: + /// + /// \verbatim + /// + /// + /// materials/my_vs.glsl + /// materials/my_fs.glsl + /// + /// + /// + /// ambient + /// fragment + /// float_array + /// 1.0 0.0 0.0 1.0 + /// + /// + /// \endverbatim + class ShaderParam + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ShaderParam(); + + /// \brief Destructor + public: ~ShaderParam() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 6b712785a9..77197e73c3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -76,6 +76,7 @@ set(tests_needing_display optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc + shader_param_system.cc thermal_system.cc thermal_sensor_system.cc ) diff --git a/test/integration/shader_param_system.cc b/test/integration/shader_param_system.cc new file mode 100644 index 0000000000..906133e937 --- /dev/null +++ b/test/integration/shader_param_system.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 +#include +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test ShaderParamTest system +class ShaderParamTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + mutex.lock(); + unsigned int channels = 3u; + unsigned int imageSamples = _msg.width() * _msg.height(); + unsigned int imageBufferSize = imageSamples * sizeof(unsigned char) + * channels; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples * channels]; + memcpy(imageBuffer, _msg.data().c_str(), imageBufferSize); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks camera image data to verify that the sphere is using +// custom material shaders +TEST_F(ShaderParamTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ShaderParam)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "shader_param.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the image camera topic + transport::Node node; + node.Subscribe("/camera", &imageCb); + + // Run server and verify that we are receiving a message + // from the image camera + size_t iters100 = 100u; + server.Run(true, iters100, false); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + // shaders set the sphere color to red + unsigned int height = 320; + unsigned int width = 240; + + int mid = (height / 2 * width * 3u) + (width / 2 - 1) * 3u; + + // Lock access to buffer and don't release it + mutex.lock(); + int r = static_cast(imageBuffer[mid]); + int g = static_cast(imageBuffer[mid+1]); + int b = static_cast(imageBuffer[mid+2]); + EXPECT_GT(r, g); + EXPECT_GT(r, b); + EXPECT_EQ(g, b); + + delete[] imageBuffer; +} diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 19306775b4..554589094a 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -13,6 +13,7 @@ set (test_plugins EventTriggerSystem TestModelSystem TestSensorSystem + TestVisualSystem TestWorldSystem MockSystem Null diff --git a/test/plugins/TestVisualSystem.cc b/test/plugins/TestVisualSystem.cc new file mode 100644 index 0000000000..9cc3741117 --- /dev/null +++ b/test/plugins/TestVisualSystem.cc @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 "TestVisualSystem.hh" + +#include + +IGNITION_ADD_PLUGIN(ignition::gazebo::TestVisualSystem, + ignition::gazebo::System, + ignition::gazebo::TestVisualSystem::ISystemConfigure) diff --git a/test/plugins/TestVisualSystem.hh b/test/plugins/TestVisualSystem.hh new file mode 100644 index 0000000000..e8cdf4a45f --- /dev/null +++ b/test/plugins/TestVisualSystem.hh @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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. + * +*/ +#ifndef IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_ +#define IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ +using VisualPluginComponent = components::Component; +IGN_GAZEBO_REGISTER_COMPONENT("VisualPluginComponent", + VisualPluginComponent) +} +} + +class TestVisualSystem : + public System, + public ISystemConfigure +{ + public: TestVisualSystem() + { + igndbg << "Constructing TestVisualSystem" << std::endl; + } + + public: ~TestVisualSystem() + { + igndbg << "Destroying TestVisualSystem" << std::endl; + } + + private: bool Service(msgs::StringMsg &_msg) + { + igndbg << "TestVisualSystem service called" << std::endl; + _msg.set_data("TestVisualSystem"); + return true; + } + + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventManager*/) override + { + igndbg << "Configuring TestVisualSystem" << std::endl; + auto value = _sdf->Get("visual_key"); + _ecm.CreateComponent(_entity, + components::VisualPluginComponent(value)); + + // Create a test service + this->node.Advertise("/test/service/visual", + &TestVisualSystem::Service, this); + } + + private: transport::Node node; +}; +} +} + +#endif diff --git a/test/worlds/plugins.sdf b/test/worlds/plugins.sdf index 466215d186..d4668e294d 100644 --- a/test/worlds/plugins.sdf +++ b/test/worlds/plugins.sdf @@ -25,6 +25,13 @@ 456 + + + 890 + + From 986e30f411fba49f60111d708a7cf4cbf5c302a7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 3 Feb 2022 17:41:53 -0800 Subject: [PATCH 08/36] Limit thruster system's input thrust cmd (#1318) * limit thrust cmd Signed-off-by: Ian Chen * check and error when max < min Signed-off-by: Ian Chen --- src/systems/thruster/Thruster.cc | 31 +++++++++++++++++++++++++++---- src/systems/thruster/Thruster.hh | 4 ++++ test/integration/thruster.cc | 22 ++++++++++++++++++++-- test/worlds/thruster_pid.sdf | 2 ++ test/worlds/thruster_vel_cmd.sdf | 2 ++ 5 files changed, 55 insertions(+), 6 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 6211434bc9..885f5ca209 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -79,12 +79,12 @@ class ignition::gazebo::systems::ThrusterPrivateData /// and writes the angular velocity directly to the joint. default: false public: bool velocityControl = false; - /// \brief Maximum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Maximum input force [N] for the propellerController, + /// default: 1000N public: double cmdMax = 1000; - /// \brief Minimum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Minimum input force [N] for the propellerController, + /// default: -1000N public: double cmdMin = -1000; /// \brief Thrust coefficient relating the propeller angular velocity to the @@ -206,6 +206,29 @@ void Thruster::Configure( enableComponent(_ecm, this->dataPtr->linkEntity); + double minThrustCmd = this->dataPtr->cmdMin; + double maxThrustCmd = this->dataPtr->cmdMax; + if (_sdf->HasElement("max_thrust_cmd")) + { + maxThrustCmd = _sdf->Get("max_thrust_cmd"); + } + if (_sdf->HasElement("min_thrust_cmd")) + { + minThrustCmd = _sdf->Get("min_thrust_cmd"); + } + if (maxThrustCmd < minThrustCmd) + { + ignerr << " must be greater than or equal to " + << ". Revert to using default values: " + << "min: " << this->dataPtr->cmdMin << ", " + << "max: " << this->dataPtr->cmdMax << std::endl; + } + else + { + this->dataPtr->cmdMax = maxThrustCmd; + this->dataPtr->cmdMin = minThrustCmd; + } + if (_sdf->HasElement("velocity_control")) { this->dataPtr->velocityControl = _sdf->Get("velocity_control"); diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index ec1a607d21..78eafb849b 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -64,6 +64,10 @@ namespace systems /// no units, defaults to 0.0] /// - - Derivative gain for joint PID controller. [Optional, /// no units, defaults to 0.0] + /// - - Maximum thrust command. [Optional, + /// defaults to 1000N] + /// - - Minimum thrust command. [Optional, + /// defaults to -1000N] /// /// ## Example /// An example configuration is installed with Gazebo. The example diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index af1ec159b8..d2333bc3f1 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -134,9 +134,24 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(pub.HasConnections()); - double force{300.0}; + // input force cmd - this should be capped to 0 + double forceCmd{-1000.0}; msgs::Double msg; - msg.set_data(force); + msg.set_data(forceCmd); + pub.Publish(msg); + + // Check no movement + fixture.Server()->Run(true, 100, false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); + EXPECT_EQ(100u, modelPoses.size()); + EXPECT_EQ(100u, propellerAngVels.size()); + modelPoses.clear(); + propellerAngVels.clear(); + + // input force cmd this should be capped to 300 + forceCmd = 1000.0; + msg.set_data(forceCmd); pub.Publish(msg); // Check movement @@ -152,6 +167,9 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(100u * sleep, modelPoses.size()); EXPECT_EQ(100u * sleep, propellerAngVels.size()); + // max allowed force + double force{300.0}; + // F = m * a // s = a * t^2 / 2 // F = m * 2 * s / t^2 diff --git a/test/worlds/thruster_pid.sdf b/test/worlds/thruster_pid.sdf index c18becc7d0..540c73170b 100644 --- a/test/worlds/thruster_pid.sdf +++ b/test/worlds/thruster_pid.sdf @@ -104,6 +104,8 @@ 0.004 1000 0.2 + 300 + 0 diff --git a/test/worlds/thruster_vel_cmd.sdf b/test/worlds/thruster_vel_cmd.sdf index f78236a8ca..1850eac8f4 100644 --- a/test/worlds/thruster_vel_cmd.sdf +++ b/test/worlds/thruster_vel_cmd.sdf @@ -103,6 +103,8 @@ 950 0.25 true + 300 + 0 From e032208c08765adc603700f4170ffbbadf411e34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 4 Feb 2022 09:36:38 +0100 Subject: [PATCH 09/36] Removed unused variables in shapes plugin (#1321) Signed-off-by: ahcorde --- src/gui/plugins/shapes/Shapes.cc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index dc00229760..be68df3105 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -37,11 +37,6 @@ namespace ignition::gazebo { class ShapesPrivate { - /// \brief Ignition communication node. - public: transport::Node node; - - /// \brief Transform control service name - public: std::string service; }; } From 756fa13630c1dc8cf11ec0e6f3f1ae82a77b6b50 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 4 Feb 2022 10:21:44 -0800 Subject: [PATCH 10/36] fix buoyancy test by increasing tol Signed-off-by: Ian Chen --- test/integration/buoyancy.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 9b2ca5d40d..b41f9e000b 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -156,7 +156,8 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RestoringMoments)) EXPECT_NEAR(maxUniformRoll, 0.11, 1e-1); EXPECT_NEAR(minUniformRoll, minGradedRoll, 1e-1); // Emperically derived - EXPECT_NEAR(minUniformRoll, -0.15, 1e-1); + // added extra tol (3e-5) after fixing center of volume's reference frame + EXPECT_NEAR(minUniformRoll, -0.15, 1e-1 + 3e-5); } ///////////////////////////////////////////////// From 7b4fcda6e33a4c977047df8795e5593d47e576fe Mon Sep 17 00:00:00 2001 From: Nick Lamprianidis Date: Sat, 5 Feb 2022 01:13:31 +0100 Subject: [PATCH 11/36] Add elevator system (#535) * Add elevator system Closes #420 Signed-off-by: Nick Lamprianidis * Limit line length for transition table Signed-off-by: Nick Lamprianidis * Add PRIVATE_INCLUDE_DIRS argument to gz_add_system Signed-off-by: Nick Lamprianidis * Add afsm and metapushkin libraries Signed-off-by: Nick Lamprianidis * Add namespace to events, actions and guards Signed-off-by: Nick Lamprianidis * Migrate to afsm Signed-off-by: Nick Lamprianidis * Fixes Signed-off-by: Nick Lamprianidis Co-authored-by: Nate Koenig --- examples/worlds/elevator.sdf | 80 + src/systems/CMakeLists.txt | 10 +- src/systems/elevator/CMakeLists.txt | 12 + src/systems/elevator/Elevator.cc | 452 ++++++ src/systems/elevator/Elevator.hh | 134 ++ src/systems/elevator/ElevatorCommonPrivate.hh | 99 ++ src/systems/elevator/ElevatorStateMachine.hh | 144 ++ .../elevator/state_machine/ActionsImpl.hh | 101 ++ .../state_machine/ElevatorStateMachineImpl.hh | 136 ++ .../elevator/state_machine/EventsImpl.hh | 79 + .../elevator/state_machine/GuardsImpl.hh | 82 ++ .../elevator/state_machine/StatesImpl.hh | 250 ++++ src/systems/elevator/utils/DoorTimer.cc | 105 ++ src/systems/elevator/utils/DoorTimer.hh | 80 + src/systems/elevator/utils/JointMonitor.cc | 102 ++ src/systems/elevator/utils/JointMonitor.hh | 79 + src/systems/elevator/vender/afsm/LICENSE | 201 +++ .../vender/afsm/include/afsm/definition.hpp | 721 +++++++++ .../afsm/include/afsm/definition_fwd.hpp | 51 + .../afsm/include/afsm/detail/actions.hpp | 503 +++++++ .../afsm/include/afsm/detail/base_states.hpp | 991 +++++++++++++ .../afsm/include/afsm/detail/debug_io.hpp | 46 + .../afsm/include/afsm/detail/def_traits.hpp | 157 ++ .../include/afsm/detail/event_identity.hpp | 52 + .../detail/exception_safety_guarantees.hpp | 46 + .../afsm/include/afsm/detail/helpers.hpp | 195 +++ .../afsm/include/afsm/detail/observer.hpp | 238 +++ .../afsm/detail/orthogonal_regions.hpp | 474 ++++++ .../include/afsm/detail/reject_policies.hpp | 45 + .../vender/afsm/include/afsm/detail/tags.hpp | 89 ++ .../afsm/include/afsm/detail/transitions.hpp | 1018 +++++++++++++ .../elevator/vender/afsm/include/afsm/fsm.hpp | 777 ++++++++++ .../vender/afsm/include/afsm/fsm_fwd.hpp | 34 + .../elevator/vender/metapushkin/LICENSE | 201 +++ .../metapushkin/include/pushkin/meta.hpp | 19 + .../include/pushkin/meta/algorithm.hpp | 736 ++++++++++ .../include/pushkin/meta/callable.hpp | 118 ++ .../include/pushkin/meta/char_sequence.hpp | 509 +++++++ .../include/pushkin/meta/function_traits.hpp | 256 ++++ .../include/pushkin/meta/functions.hpp | 39 + .../include/pushkin/meta/index_tuple.hpp | 38 + .../include/pushkin/meta/integer_sequence.hpp | 175 +++ .../include/pushkin/meta/nth_type.hpp | 33 + .../include/pushkin/meta/type_map.hpp | 90 ++ .../include/pushkin/meta/type_tuple.hpp | 44 + .../include/pushkin/util/demangle.hpp | 94 ++ test/integration/CMakeLists.txt | 1 + test/integration/elevator_system.cc | 113 ++ test/worlds/elevator.sdf | 1284 +++++++++++++++++ 49 files changed, 11332 insertions(+), 1 deletion(-) create mode 100644 examples/worlds/elevator.sdf create mode 100644 src/systems/elevator/CMakeLists.txt create mode 100644 src/systems/elevator/Elevator.cc create mode 100644 src/systems/elevator/Elevator.hh create mode 100644 src/systems/elevator/ElevatorCommonPrivate.hh create mode 100644 src/systems/elevator/ElevatorStateMachine.hh create mode 100644 src/systems/elevator/state_machine/ActionsImpl.hh create mode 100644 src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh create mode 100644 src/systems/elevator/state_machine/EventsImpl.hh create mode 100644 src/systems/elevator/state_machine/GuardsImpl.hh create mode 100644 src/systems/elevator/state_machine/StatesImpl.hh create mode 100644 src/systems/elevator/utils/DoorTimer.cc create mode 100644 src/systems/elevator/utils/DoorTimer.hh create mode 100644 src/systems/elevator/utils/JointMonitor.cc create mode 100644 src/systems/elevator/utils/JointMonitor.hh create mode 100644 src/systems/elevator/vender/afsm/LICENSE create mode 100644 src/systems/elevator/vender/afsm/include/afsm/definition.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/definition_fwd.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/actions.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/base_states.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/debug_io.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/def_traits.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/event_identity.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/exception_safety_guarantees.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/helpers.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/observer.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/orthogonal_regions.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/reject_policies.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/tags.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/fsm.hpp create mode 100644 src/systems/elevator/vender/afsm/include/afsm/fsm_fwd.hpp create mode 100644 src/systems/elevator/vender/metapushkin/LICENSE create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/algorithm.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/callable.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/char_sequence.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/function_traits.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/functions.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/index_tuple.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/integer_sequence.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/nth_type.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/type_map.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/meta/type_tuple.hpp create mode 100644 src/systems/elevator/vender/metapushkin/include/pushkin/util/demangle.hpp create mode 100644 test/integration/elevator_system.cc create mode 100644 test/worlds/elevator.sdf diff --git a/examples/worlds/elevator.sdf b/examples/worlds/elevator.sdf new file mode 100644 index 0000000000..d2b1238e6b --- /dev/null +++ b/examples/worlds/elevator.sdf @@ -0,0 +1,80 @@ + + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + https://fuel.ignitionrobotics.org/1.0/nlamprian/models/Elevator + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index aa4b5ce018..574319fb26 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -21,7 +21,7 @@ function(gz_add_system system_name) set(options) set(oneValueArgs) - set(multiValueArgs SOURCES PUBLIC_LINK_LIBS PRIVATE_LINK_LIBS PRIVATE_COMPILE_DEFS) + set(multiValueArgs SOURCES PUBLIC_LINK_LIBS PRIVATE_INCLUDE_DIRS PRIVATE_LINK_LIBS PRIVATE_COMPILE_DEFS) cmake_parse_arguments(gz_add_system "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) @@ -47,6 +47,13 @@ function(gz_add_system system_name) ignition-plugin${IGN_PLUGIN_VER}::register ) + if(gz_add_system_PRIVATE_INCLUDE_DIRS) + target_include_directories(${system_target} + PRIVATE + ${gz_add_system_PRIVATE_INCLUDE_DIRS} + ) + endif() + if(gz_add_system_PRIVATE_COMPILE_DEFS) target_compile_definitions(${system_target} PRIVATE @@ -82,6 +89,7 @@ add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) add_subdirectory(diff_drive) +add_subdirectory(elevator) add_subdirectory(imu) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) diff --git a/src/systems/elevator/CMakeLists.txt b/src/systems/elevator/CMakeLists.txt new file mode 100644 index 0000000000..054ac3fc8b --- /dev/null +++ b/src/systems/elevator/CMakeLists.txt @@ -0,0 +1,12 @@ +gz_add_system(elevator + SOURCES + Elevator.cc + utils/DoorTimer.cc + utils/JointMonitor.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} + PRIVATE_INCLUDE_DIRS + vender/afsm/include + vender/metapushkin/include +) diff --git a/src/systems/elevator/Elevator.cc b/src/systems/elevator/Elevator.cc new file mode 100644 index 0000000000..d78bd26bb4 --- /dev/null +++ b/src/systems/elevator/Elevator.cc @@ -0,0 +1,452 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include +#include +#include +#include +#include +#include + +#include "Elevator.hh" +#include "ElevatorCommonPrivate.hh" +#include "ElevatorStateMachine.hh" +#include "utils/DoorTimer.hh" +#include "utils/JointMonitor.hh" + +#include +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointVelocity.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +class ElevatorPrivate : public ElevatorCommonPrivate +{ + /// \brief Destructor + public: virtual ~ElevatorPrivate() override; + + /// \brief Initializes the cabin of the elevator + /// \param[in] _cabinJointName Name of the cabin joint + /// \param[in] _floorLinkPrefix Name prefix of the floor links + /// \param[in] _topicPrefix Topic prefix for the command publisher + /// \param[in] _ecm Entity component manager + /// \return True on successful initialization, or false otherwise + public: bool InitCabin(const std::string &_cabinJointName, + const std::string &_floorLinkPrefix, + const std::string &_topicPrefix, + EntityComponentManager &_ecm); + + /// \brief Initializes the doors of the elevator + /// \param[in] _doorJointPrefix Name prefix of the door joints + /// \param[in] _topicPrefix Topic prefix for the command publishers + /// \param[in] _ecm Entity component manager + /// \return True on successful initialization, or false otherwise + public: bool InitDoors(const std::string &_doorJointPrefix, + const std::string &_topicPrefix, + EntityComponentManager &_ecm); + + // Documentation inherited + public: virtual void StartDoorTimer( + int32_t _floorTarget, + const std::function &_timeoutCallback) override; + + // Documentation inherited + public: virtual void SetDoorMonitor( + int32_t _floorTarget, double _jointTarget, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback) override; + + // Documentation inherited + public: virtual void SetCabinMonitor( + int32_t _floorTarget, double _jointTarget, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback) override; + + /// \brief Updates the elevator state based on the current cabin position and + /// then publishes the new state + /// \param[in] _info Current simulation step info + /// \param[in] _ecm Entity component manager + public: void UpdateState(const ignition::gazebo::UpdateInfo &_info, + const EntityComponentManager &_ecm); + + /// \brief Callback for the door lidar scans + /// \param[in] _floorLevel Floor level + /// \param[in] _msg Laserscan message + public: void OnLidarMsg(size_t _floorLevel, const msgs::LaserScan &_msg); + + /// \brief Callback for the elevator commands + /// \param[in] _msg Message that carries the floor level target + public: void OnCmdMsg(const msgs::Int32 &_msg); + + /// \brief Ignition communication node + public: transport::Node node; + + /// \brief Model to which this system belongs + public: Model model; + + /// \brief Joints of the doors of the elevator + public: std::vector doorJoints; + + /// \brief Joint of the cabin + public: Entity cabinJoint; + + /// \brief State vector that identifies whether the doorway on each floor + /// level is blocked + public: std::vector isDoorwayBlockedStates; + + /// \brief Timer that keeps the door at the target floor level open + public: std::unique_ptr doorTimer; + + /// \brief Monitor that checks whether the door at the target floor level has + /// been opened or closed + public: JointMonitor doorJointMonitor; + + /// \brief Monitor that checks whether the cabin has reached the target floor + /// level + public: JointMonitor cabinJointMonitor; + + /// \brief System update period calculated from + public: std::chrono::steady_clock::duration updatePeriod{0}; + + /// \brief Last system update simulation time + public: std::chrono::steady_clock::duration lastUpdateTime{0}; + + /// \brief State publish period calculated from + public: std::chrono::steady_clock::duration statePubPeriod{0}; + + /// \brief Last state publish simulation time + public: std::chrono::steady_clock::duration lastStatePubTime{0}; + + /// \brief Elevator state publisher + public: transport::Node::Publisher statePub; + + /// \brief Elevator state message + public: msgs::Int32 stateMsg; + + /// \brief Elevator state machine + public: std::unique_ptr stateMachine; +}; + +////////////////////////////////////////////////// +Elevator::Elevator() : dataPtr(std::make_shared()) {} + +////////////////////////////////////////////////// +void Elevator::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager & /*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + // Initialize system update period + double rate = _sdf->Get("update_rate", 10).first; + std::chrono::duration period{rate > 0 ? 1 / rate : 0}; + this->dataPtr->updatePeriod = + std::chrono::duration_cast(period); + + // Get floor link prefix + std::string floorLinkPrefix = + _sdf->Get("floor_link_prefix", "floor_").first; + + // Get door joint prefix + std::string doorJointPrefix = + _sdf->Get("door_joint_prefix", "door_").first; + + // Get cabin joint name + std::string cabinJointName = + _sdf->Get("cabin_joint", "lift").first; + + std::string topicPrefix = "/model/" + this->dataPtr->model.Name(_ecm); + + if (!this->dataPtr->InitCabin(cabinJointName, floorLinkPrefix, topicPrefix, + _ecm)) + return; + + if (!this->dataPtr->InitDoors(doorJointPrefix, topicPrefix, _ecm)) + return; + + // Initialize door timer + double duration = _sdf->Get("open_door_wait_duration", 5.0).first; + this->dataPtr->doorTimer = std::make_unique( + std::chrono::duration_cast( + std::chrono::duration(duration))); + + // Initialize state publisher + std::string stateTopicName = + _sdf->Get("state_topic", topicPrefix + "/state").first; + // NOTE Topic should be latched; no latch option so far + this->dataPtr->statePub = + this->dataPtr->node.Advertise(stateTopicName); + + // Initialize state publish period + double stateRate = _sdf->Get("state_publish_rate", 5.0).first; + std::chrono::duration statePeriod{stateRate > 0 ? 1 / stateRate : 0}; + this->dataPtr->statePubPeriod = + std::chrono::duration_cast( + statePeriod); + + // Initialize state machine + this->dataPtr->stateMachine = + std::make_unique(this->dataPtr); + + // Subscribe to command topic + std::string cmdTopicName = + _sdf->Get("cmd_topic", topicPrefix + "/cmd").first; + this->dataPtr->node.Subscribe(cmdTopicName, &ElevatorPrivate::OnCmdMsg, + this->dataPtr.get()); + ignmsg << "System " << this->dataPtr->model.Name(_ecm) << " subscribed to " + << cmdTopicName << " for command messages" << std::endl; +} + +////////////////////////////////////////////////// +void Elevator::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("Elevator::PostUpdate"); + if (_info.paused) return; + + // Throttle update rate + auto elapsed = _info.simTime - this->dataPtr->lastUpdateTime; + if (elapsed > std::chrono::steady_clock::duration::zero() && + elapsed < this->dataPtr->updatePeriod) + return; + this->dataPtr->lastUpdateTime = _info.simTime; + + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->UpdateState(_info, _ecm); + this->dataPtr->doorTimer->Update( + _info, this->dataPtr->isDoorwayBlockedStates[this->dataPtr->state]); + this->dataPtr->doorJointMonitor.Update(_ecm); + this->dataPtr->cabinJointMonitor.Update(_ecm); +} + +////////////////////////////////////////////////// +ElevatorPrivate::~ElevatorPrivate() +{ +} + +////////////////////////////////////////////////// +bool ElevatorPrivate::InitCabin(const std::string &_cabinJointName, + const std::string &_floorLinkPrefix, + const std::string &_topicPrefix, + EntityComponentManager &_ecm) +{ + // Validate and initialize cabin joint + this->cabinJoint = this->model.JointByName(_ecm, _cabinJointName); + if (this->cabinJoint == kNullEntity) + { + ignerr << "Failed to find cabin joint " << _cabinJointName << std::endl; + return false; + } + if (!_ecm.EntityHasComponentType(this->cabinJoint, + components::JointPosition().TypeId())) + _ecm.CreateComponent(this->cabinJoint, components::JointPosition()); + if (!_ecm.EntityHasComponentType(this->cabinJoint, + components::JointVelocity().TypeId())) + _ecm.CreateComponent(this->cabinJoint, components::JointVelocity()); + + // Initialize cabin floor targets + size_t numFloorLinks = 0; + std::regex floorLinkRegex(_floorLinkPrefix + "\\d+"); + std::vector links = + _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); + for (const auto &link : links) + { + auto name = _ecm.Component(link)->Data(); + numFloorLinks += std::regex_match(name, floorLinkRegex); + } + for (size_t i = 0; i < numFloorLinks; ++i) + { + auto name = _floorLinkPrefix + std::to_string(i); + auto link = this->model.LinkByName(_ecm, name); + if (link == kNullEntity) + { + ignerr << "Failed to find floor link " << name << std::endl; + return false; + } + auto z = _ecm.Component(link)->Data().Z(); + this->cabinTargets.push_back(z); + } + + // Initialize cabin joint command publisher + std::string cabinJointCmdTopicName = + _topicPrefix + "/joint/" + _cabinJointName + "/0/cmd_pos"; + this->cabinJointCmdPub = + this->node.Advertise(cabinJointCmdTopicName); + + return true; +} + +////////////////////////////////////////////////// +bool ElevatorPrivate::InitDoors(const std::string &_doorJointPrefix, + const std::string &_topicPrefix, + EntityComponentManager &_ecm) +{ + for (size_t i = 0; i < this->cabinTargets.size(); ++i) + { + // Validate and initialize door joint + auto name = _doorJointPrefix + std::to_string(i); + auto joint = this->model.JointByName(_ecm, name); + if (joint == kNullEntity) + { + ignerr << "Failed to find door joint " << name << std::endl; + return false; + } + if (!_ecm.EntityHasComponentType(joint, + components::JointPosition().TypeId())) + _ecm.CreateComponent(joint, components::JointPosition()); + if (!_ecm.EntityHasComponentType(joint, + components::JointVelocity().TypeId())) + _ecm.CreateComponent(joint, components::JointVelocity()); + this->doorJoints.push_back(joint); + + // Initialize open door target + auto upper = _ecm.Component(joint)->Data().Upper(); + this->doorTargets.push_back(upper); + + // Initialize door joint command publisher + std::string topicName = _topicPrefix + "/joint/" + name + "/0/cmd_pos"; + auto pub = this->node.Advertise(topicName); + this->doorJointCmdPubs.push_back(pub); + } + + // Initialize blocked doorway states + this->isDoorwayBlockedStates = + std::vector(this->doorJoints.size(), false); + + // Subscribe to lidar topics + for (size_t i = 0; i < this->doorJoints.size(); ++i) + { + auto jointName = _doorJointPrefix + std::to_string(i); + std::string topicName = _topicPrefix + "/" + jointName + "/lidar"; + std::function callback = + std::bind(&ElevatorPrivate::OnLidarMsg, this, i, std::placeholders::_1); + this->node.Subscribe(topicName, callback); + } + + return true; +} + +////////////////////////////////////////////////// +void ElevatorPrivate::StartDoorTimer( + int32_t /*_floorTarget*/, const std::function &_timeoutCallback) +{ + std::lock_guard lock(this->mutex); + this->doorTimer->Configure(this->lastUpdateTime, _timeoutCallback); +} + +////////////////////////////////////////////////// +void ElevatorPrivate::SetDoorMonitor( + int32_t _floorTarget, double _jointTarget, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback) +{ + std::lock_guard lock(this->mutex); + this->doorJointMonitor.Configure(this->doorJoints[_floorTarget], _jointTarget, + _posEps, _velEps, + _jointTargetReachedCallback); +} + +////////////////////////////////////////////////// +void ElevatorPrivate::SetCabinMonitor( + int32_t /*_floorTarget*/, double _jointTarget, double _posEps, + double _velEps, const std::function &_jointTargetReachedCallback) +{ + std::lock_guard lock(this->mutex); + this->cabinJointMonitor.Configure(this->cabinJoint, _jointTarget, _posEps, + _velEps, _jointTargetReachedCallback); +} + +////////////////////////////////////////////////// +void ElevatorPrivate::UpdateState(const ignition::gazebo::UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + // Update state + double pos = + _ecm.ComponentData(this->cabinJoint)->front(); + std::vector diffs(this->cabinTargets.size()); + std::transform(this->cabinTargets.begin(), this->cabinTargets.end(), + diffs.begin(), + [&pos](auto target) { return std::fabs(target - pos); }); + auto it = std::min_element(diffs.begin(), diffs.end()); + this->state = static_cast(std::distance(diffs.begin(), it)); + + // Throttle publish rate + auto elapsed = _info.simTime - this->lastStatePubTime; + if (elapsed > std::chrono::steady_clock::duration::zero() && + elapsed < this->statePubPeriod) + return; + this->lastStatePubTime = _info.simTime; + + this->stateMsg.set_data(this->state); + this->statePub.Publish(this->stateMsg); +} + +////////////////////////////////////////////////// +void ElevatorPrivate::OnLidarMsg(size_t _floorLevel, + const msgs::LaserScan &_msg) +{ + bool isDoorwayBlocked = _msg.ranges(0) < _msg.range_max() - 0.005; + if (isDoorwayBlocked == this->isDoorwayBlockedStates[_floorLevel]) return; + std::lock_guard lock(this->mutex); + this->isDoorwayBlockedStates[_floorLevel] = isDoorwayBlocked; +} + +////////////////////////////////////////////////// +void ElevatorPrivate::OnCmdMsg(const msgs::Int32 &_msg) +{ + auto target = _msg.data(); + if (target < 0 || target >= static_cast(this->cabinTargets.size())) + { + ignwarn << "Invalid target [" << target << "]; command must be in [0," + << this->cabinTargets.size() << ")" << std::endl; + return; + } + this->stateMachine->process_event(events::EnqueueNewTarget(_msg.data())); +} + +IGNITION_ADD_PLUGIN(Elevator, System, Elevator::ISystemConfigure, + Elevator::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(Elevator, "ignition::gazebo::systems::Elevator") + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/Elevator.hh b/src/systems/elevator/Elevator.hh new file mode 100644 index 0000000000..cbb44da5b7 --- /dev/null +++ b/src/systems/elevator/Elevator.hh @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_ELEVATOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ELEVATOR_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +// Data forward declaration +class ElevatorPrivate; + +/// \brief System that controls an elevator. It closely models the structure +/// and functionality of a real elevator. It individually controls the cabin +/// and doors of the elevator, queues commands for the elevator and stops at +/// intermediate floors if a command is received while the last one is ongoing, +/// and keeps a door open if the doorway is blocked. The model of the elevator +/// can have arbitrarily many floor levels and at arbitrary heights each. +/// +/// ## Assumptions on the Elevator Model +/// +/// In the default configuration, the cabin is at the ground floor and the +/// doors are closed +/// +/// In the default configuration, the cabin and door joints are at zero position +/// +/// There are reference floor links along the cabin joint that indicate the +/// cabin joint positions for each floor. The names of links follow the pattern +/// indicated by `` +/// +/// There is a door in each floor and the names of the doors follow the pattern +/// indicated by `` +/// +/// Each cabin and door joint has an associated joint position controller +/// system that listens for command to +/// `/model/{model_name}/joint/{joint_name}/0/cmd_pos` +/// +/// Each door (optionally) has a lidar that, if intercepted, indicates that the +/// doorway is blocked. The lidar publishes sensor data on topic +/// `/model/{model_name}/{door_joint_name}/lidar` +/// +/// ## System Parameters +/// +/// ``: System update rate. This element is optional and the +/// default value is 10Hz. A value of zero gets translated to the simulation +/// rate (no throttling for the system). +/// +/// ``: Prefix in the names of the links that function as +/// a reference for each floor level. When the elevator is requested to move +/// to a given floor level, the cabin is commanded to move to the height of +/// the corresponding floor link. The names of the links will be expected to +/// be `{prerix}i`, where \f$i=[0,N)\f$ and N is the number of floors. This +/// element is optional and the default value is `floor_`. +/// +/// ``: Prefix in the names of the joints that control the +/// doors of the elevator. The names of the joints will be expected to be +/// `{prerix}i`, where \f$i=[0,N)\f$ and N is the number of floors. This +/// element is optional and the default value is `door_`. +/// +/// ``: Name of the joint that controls the position of the +/// cabin. This element is optional and the default value is `lift`. +/// +/// ``: Topic to which this system will subscribe in order to +/// receive command messages. This element is optional and the default value +/// is `/model/{model_name}/cmd`. +/// +/// ``: Topic on which this system will publish state (current +/// floor) messages. This element is optional and the default value is +/// `/model/{model_name}/state`. +/// +/// ``: State publication rate. This rate is bounded by +/// ``. This element is optional and the default value is 5Hz. +/// +/// ``: Time to wait with a door open before the door +/// closes. This element is optional and the default value is 5 sec. +class IGNITION_GAZEBO_VISIBLE Elevator : public System, + public ISystemConfigure, + public ISystemPostUpdate +{ + /// \brief Constructor + public: Elevator(); + + /// \brief Destructor + public: ~Elevator() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::shared_ptr dataPtr; +}; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_SYSTEMS_ELEVATOR_HH_ diff --git a/src/systems/elevator/ElevatorCommonPrivate.hh b/src/systems/elevator/ElevatorCommonPrivate.hh new file mode 100644 index 0000000000..ec787e3b91 --- /dev/null +++ b/src/systems/elevator/ElevatorCommonPrivate.hh @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_ELEVATOR_COMMON_PRIVATE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ELEVATOR_COMMON_PRIVATE_HH_ + +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +class ElevatorCommonPrivate +{ + /// \brief Destructor + public: virtual ~ElevatorCommonPrivate() = default; + + /// \brief Starts the timer that keeps the door at the target floor level open + /// \param[in] _floorTarget Target floor level + /// \param[in] _timeoutCallback Function to call upon timeout + public: virtual void StartDoorTimer( + int32_t _floorTarget, const std::function &_timeoutCallback) = 0; + + /// \brief Configures the monitor that checks the state of the joint of the + /// door at the target floor level + /// \param[in] _floorTarget Target floor level + /// \param[in] _jointTarget Last joint target (command) + /// \param[in] _posEps Position tolerance + /// \param[in] _velEps Velocity tolerance + /// \param[in] _jointTargetReachedCallback Function to call when the door + /// joint reaches its target + public: virtual void SetDoorMonitor( + int32_t _floorTarget, double _jointTarget, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback) = 0; + + /// \brief Configures the monitor that checks the state of the cabin joint + /// \param[in] _floorTarget Target floor level + /// \param[in] _jointTarget Last joint target (command) + /// \param[in] _posEps Position tolerance + /// \param[in] _velEps Velocity tolerance + /// \param[in] _jointTargetReachedCallback Function to call when the cabin + /// joint reaches its target + public: virtual void SetCabinMonitor( + int32_t _floorTarget, double _jointTarget, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback) = 0; + + /// \brief Door joint command publishers + public: std::vector doorJointCmdPubs; + + /// \brief Cabin joint command publisher + public: transport::Node::Publisher cabinJointCmdPub; + + /// \brief Joint commands for opening the door at each floor level + public: std::vector doorTargets; + + /// \brief Cabin joint commands for each floor level + public: std::vector cabinTargets; + + /// \brief Current floor at which the cabin is + public: int32_t state{0}; + + /// \brief A mutex to protect access to targets and state + public: std::recursive_mutex mutex; +}; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_SYSTEMS_ELEVATOR_COMMON_PRIVATE_HH_ diff --git a/src/systems/elevator/ElevatorStateMachine.hh b/src/systems/elevator/ElevatorStateMachine.hh new file mode 100644 index 0000000000..6dfa7811f3 --- /dev/null +++ b/src/systems/elevator/ElevatorStateMachine.hh @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_ELEVATOR_STATE_MACHINE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ELEVATOR_STATE_MACHINE_HH_ + +#include + +#include +#include + +#include "afsm/fsm.hpp" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +// Data forward declarations +class ElevatorCommonPrivate; +class ElevatorStateMachinePrivate; + +// Event forward declarations +namespace events +{ + struct EnqueueNewTarget; + struct NewTarget; + struct DoorOpen; + struct DoorClosed; + struct Timeout; + struct CabinAtTarget; +} // namespace events + +// Action forward declarations +namespace actions +{ + template + struct EnqueueNewTarget; + struct NewTarget; + struct CabinAtTarget; +} // namespace actions + +// Guard forward declarations +namespace guards +{ + template + struct IsInState; + struct CabinAtTarget; + struct NoQueuedTarget; +} // namespace guards + +/// \brief Elevator state machine frontend. Defines the transition table and +/// initial state of the state machine. +class ElevatorStateMachineDef + : public ::afsm::def::state_machine +{ + // State forward declarations + struct IdleState; + template + struct DoorState; + struct OpenDoorState; + struct CloseDoorState; + struct WaitState; + struct MoveCabinState; + + /// \brief Constructor + /// \param[in] _system Data that are common to both the system and the state + /// machine. + public: ElevatorStateMachineDef( + const std::shared_ptr &_system); + + /// \brief Destructor + public: ~ElevatorStateMachineDef(); + + /// \brief Initial state of the state machine + public: using initial_state = IdleState; + + /// \brief Transition transition table + public: using internal_transitions = transition_table < + in, + guards::IsInState >, + in, + guards::IsInState > + >; + + /// \brief Transition table + public: using transitions = transition_table< + // +--------------------------------------------------------------+ + tr, + tr >, + // +--------------------------------------------------------------+ + tr, + // +--------------------------------------------------------------+ + tr, + // +--------------------------------------------------------------+ + tr, + tr >, + // +--------------------------------------------------------------+ + tr + >; + + /// \brief Public data pointer + public: std::unique_ptr dataPtr; +}; + +/// \brief Elevator state machine backend +using ElevatorStateMachine = ::afsm::state_machine; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#include "state_machine/ElevatorStateMachineImpl.hh" + +#endif // IGNITION_GAZEBO_SYSTEMS_ELEVATOR_STATE_MACHINE_HH_ diff --git a/src/systems/elevator/state_machine/ActionsImpl.hh b/src/systems/elevator/state_machine/ActionsImpl.hh new file mode 100644 index 0000000000..e613fc6388 --- /dev/null +++ b/src/systems/elevator/state_machine/ActionsImpl.hh @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include "../ElevatorStateMachine.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +namespace actions +{ +/// \brief Action that enqueues a new target in the target queue. +/// \details After the new target has been added in the queue, depending on the +/// template parameter, an events::NewTarget is triggered. +template +struct EnqueueNewTarget +{ + /// \brief Function call operator + /// \param[in] _event Event that triggered the action + /// \param[in] _fsm State machine with which the action is associated + /// \param[in] _source Source state + /// \param[in] _target Target state + public: template + void operator()(const Event &_event, Fsm &_fsm, Source & /*_source*/, + Target & /*_target*/) + { + _fsm.dataPtr->EnqueueNewTarget(_event.target); + if (trigger) + _fsm.process_event(events::NewTarget()); + } +}; + +/// \brief Action that cleans up the target queue when a new target is +/// processed. +struct NewTarget +{ + /// \brief Function call operator + /// \param[in] _event Event that triggered the action + /// \param[in] _fsm State machine with which the action is associated + /// \param[in] _source Source state + /// \param[in] _target Target state + public: template + void operator()(const Event & /*_event*/, Fsm &_fsm, Source & /*_source*/, + Target & /*_target*/) + { + std::lock_guard lock(_fsm.dataPtr->system->mutex); + if (_fsm.dataPtr->targets.front() == _fsm.dataPtr->system->state) + _fsm.dataPtr->targets.pop_front(); + } +}; + +/// \brief Action that cleans up the target queue when the cabin reaches the +/// target floor level. +struct CabinAtTarget +{ + /// \brief Function call operator + /// \param[in] _event Event that triggered the action + /// \param[in] _fsm State machine with which the action is associated + /// \param[in] _source Source state + /// \param[in] _target Target state + public: template + void operator()(const Event & /*_event*/, Fsm &_fsm, Source & /*_source*/, + Target & /*_target*/) + { + std::lock_guard lock(_fsm.dataPtr->system->mutex); + _fsm.dataPtr->targets.pop_front(); + } +}; + +} // namespace actions +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh b/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh new file mode 100644 index 0000000000..a995a69ecd --- /dev/null +++ b/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include +#include +#include +#include + +#include + +#include "../ElevatorStateMachine.hh" + +#include "../ElevatorCommonPrivate.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +class ElevatorStateMachinePrivate +{ + /// \brief Constructor + /// \param[in] _system Data of the enclosing system + public: ElevatorStateMachinePrivate( + const std::shared_ptr &_system); + + /// \brief Adds a new target to the queue + /// \details If there is another target in the queue, and the new target is + /// an intermediate stop, the new target will take precedence + /// \param[in] _target Target to add to the queue + public: void EnqueueNewTarget(double _target); + + /// \brief Publishes a command message + /// \param[in] _pub Joint command publisher + /// \param[in] _cmd Joint command + public: void SendCmd(transport::Node::Publisher &_pub, double _cmd); + + /// \brief Data of the enclosing system + public: std::shared_ptr system; + + /// \brief Floor target queue + public: std::deque targets; +}; + +////////////////////////////////////////////////// +ElevatorStateMachinePrivate::ElevatorStateMachinePrivate( + const std::shared_ptr &_system) + : system(_system) +{ +} + +////////////////////////////////////////////////// +void ElevatorStateMachinePrivate::EnqueueNewTarget(double _target) +{ + // Ignore duplicate targets + auto it = std::find(this->targets.cbegin(), this->targets.cend(), _target); + if (it != this->targets.cend()) + return; + + // Prioritize target in the queue + bool enqueued = false; + int32_t prevTarget = this->system->state; + for (it = this->targets.cbegin(); it != this->targets.cend(); ++it) + { + int32_t lower = prevTarget; + int32_t upper = *it; + if (upper < lower) std::swap(lower, upper); + if (_target >= lower && _target < upper) + { + this->targets.insert(it, _target); + enqueued = true; + break; + } + prevTarget = *it; + } + if (!enqueued) + this->targets.push_back(_target); + + std::ostringstream ss; + ss << "The elevator enqueued target " << _target << " [ "; + for (const auto &target : this->targets) ss << target << " "; + ignmsg << ss.str() << "]" << std::endl; +} + +////////////////////////////////////////////////// +void ElevatorStateMachinePrivate::SendCmd(transport::Node::Publisher &_pub, + double _cmd) +{ + msgs::Double msg; + msg.set_data(_cmd); + _pub.Publish(msg); +} + +////////////////////////////////////////////////// +ElevatorStateMachineDef::ElevatorStateMachineDef( + const std::shared_ptr &_system) + : dataPtr(std::make_unique(_system)) +{ +} + +////////////////////////////////////////////////// +ElevatorStateMachineDef::~ElevatorStateMachineDef() = default; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#include "EventsImpl.hh" + +#include "ActionsImpl.hh" +#include "GuardsImpl.hh" +#include "StatesImpl.hh" diff --git a/src/systems/elevator/state_machine/EventsImpl.hh b/src/systems/elevator/state_machine/EventsImpl.hh new file mode 100644 index 0000000000..1aa4d9a781 --- /dev/null +++ b/src/systems/elevator/state_machine/EventsImpl.hh @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +namespace events +{ +/// \brief Event that signifies there is a new target that needs to be enqueued. +struct EnqueueNewTarget +{ + /// \brief Constructor + /// \param[in] _target New target + public: EnqueueNewTarget(double _target) : target(_target) + { + } + + /// \brief target New target + public: double target; +}; + +/// \brief Event that signifies a new target will be processed. +struct NewTarget +{ +}; + +/// \brief Event that signifies the door at the target floor level has been +/// opened. +struct DoorOpen +{ +}; + +/// \brief Event that signifies the door at the target floor level has been +/// closed. +struct DoorClosed +{ +}; + +/// \brief Event that signifies the door at the target floor level has remained +/// open for the required amount of time. +struct Timeout +{ +}; + +/// \brief Event that signifies the cabin has reached the target floor level. +struct CabinAtTarget +{ +}; + +} // namespace events +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/state_machine/GuardsImpl.hh b/src/systems/elevator/state_machine/GuardsImpl.hh new file mode 100644 index 0000000000..ec5ac4c706 --- /dev/null +++ b/src/systems/elevator/state_machine/GuardsImpl.hh @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +namespace guards +{ +/// \brief Guard that checks whether the state machine is in a given state. +/// \note The template parameter generalizes the target state. +template +struct IsInState +{ + /// \brief Function call operator + /// \param[in] _fsm State machine with which the guard is associated + public: template + bool operator()(const Fsm &_fsm, const State &) + { + // NOTE Mind the inversion; the internal transition table needs + // the guard to return false in order to trigger a transition + return !_fsm.template is_in_state(); + } +}; + +/// \brief Guard that checks whether the cabin is at the target floor level. +struct CabinAtTarget +{ + /// \brief Function call operator + /// \param[in] _fsm State machine with which the guard is associated + public: template + bool operator()(const Fsm &_fsm, const State &) + { + std::lock_guard lock(_fsm.dataPtr->system->mutex); + return _fsm.dataPtr->targets.front() == _fsm.dataPtr->system->state; + } +}; + +/// \brief Guard that checks whether the target queue is empty. +struct NoQueuedTarget +{ + /// \brief Function call operator + /// \param[in] _fsm State machine with which the guard is associated + public: template + bool operator()(const Fsm &_fsm, const State &) + { + std::lock_guard lock(_fsm.dataPtr->system->mutex); + return _fsm.dataPtr->targets.empty(); + } +}; + +} // namespace guards +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/state_machine/StatesImpl.hh b/src/systems/elevator/state_machine/StatesImpl.hh new file mode 100644 index 0000000000..accd7f4a47 --- /dev/null +++ b/src/systems/elevator/state_machine/StatesImpl.hh @@ -0,0 +1,250 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include +#include + +#include "../ElevatorStateMachine.hh" + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +/// \brief State at which the elevator is idling. +struct ElevatorStateMachineDef::IdleState : state +{ + /// \brief Logs the entry to the state + public: template + void on_enter(const Event &, FSM &) + { + ignmsg << "The elevator is idling" << std::endl; + } +}; + +/// \brief Virtual state at which the elevator is opening or closing a door. +template +struct ElevatorStateMachineDef::DoorState : state> +{ + /// \brief Constructor + /// \param[in] _posEps Position tolerance when checking if a door has opened + /// or closed + /// \param[in] _velEps Velocity tolerance when checking if a door has opened + /// or closed + public: DoorState(double _posEps = 2e-2, double _velEps = 1e-2) + : posEps(_posEps), velEps(_velEps) + { + } + + /// \brief Destructor + public: virtual ~DoorState() + { + } + + /// \brief Sends the opening or closing command to the door joint of the + /// target floor and configures the monitor that checks the joint state + /// \param[in] _fsm State machine to which the state belongs + public: template + void on_enter(const Event &, FSM &_fsm) + { + const auto &data = _fsm.dataPtr; + int32_t floorTarget = data->system->state; + ignmsg << "The elevator is " << this->Report(data) << std::endl; + + double jointTarget = this->JointTarget(data, floorTarget); + data->SendCmd(data->system->doorJointCmdPubs[floorTarget], jointTarget); + this->triggerEvent = [&_fsm] { _fsm.process_event(E()); }; + data->system->SetDoorMonitor( + floorTarget, jointTarget, this->posEps, this->velEps, + std::bind(&DoorState::OnJointTargetReached, this)); + } + + /// \brief Gets the message that's being reported when entering the state + /// \param[in] _data State machine data + /// \return String with the message the state has to report + private: virtual std::string Report( + const std::unique_ptr &_data) const = 0; + + /// \brief Gets the joint position for opening or closing a door + /// \param[in] _data State machine data + /// \param[in] _floorTarget Target floor level + /// \return Joint position + private: virtual double JointTarget( + const std::unique_ptr &_data, + int32_t _floorTarget) const = 0; + + /// \brief Method that gets called when a door joint reaches its target + private: void OnJointTargetReached() + { + this->triggerEvent(); + } + + /// \brief Positiion tolerance + private: double posEps; + + /// \brief Velocity tolerance + private: double velEps; + + /// \brief Triggers the exit event + private: std::function triggerEvent; +}; + +/// \brief State at which the elevator is opening a door. +struct ElevatorStateMachineDef::OpenDoorState : DoorState +{ + /// \brief This state defers EnqueueNewTargetEvent events + public: using deferred_events = type_tuple; + + /// \brief Constructor + public: OpenDoorState() = default; + + // Documentation inherited + private: virtual std::string Report( + const std::unique_ptr &_data) const override + { + return "opening door " + std::to_string(_data->system->state); + } + + // Documentation inherited + private: virtual double JointTarget( + const std::unique_ptr &_data, + int32_t _floorTarget) const override + { + return _data->system->doorTargets[_floorTarget]; + } +}; + +/// \brief State at which the elevator is closing a door. +struct ElevatorStateMachineDef::CloseDoorState : DoorState +{ + /// \brief Constructor + public: CloseDoorState() = default; + + // Documentation inherited + private: virtual std::string Report( + const std::unique_ptr &_data) const override + { + return "closing door " + std::to_string(_data->system->state); + } + + // Documentation inherited + private: virtual double JointTarget( + const std::unique_ptr & /*_data*/, + int32_t /*_floorTarget*/) const override + { + return 0.0; + } +}; + +/// \brief State at which the elevator is waiting with a door open. +struct ElevatorStateMachineDef::WaitState : state +{ + /// \brief This state defers EnqueueNewTargetEvent events + public: using deferred_events = type_tuple; + + /// \brief Starts the timer that keeps the door at the target floor level open + /// \param[in] _fsm State machine to which the state belongs + public: template + void on_enter(const Event &, FSM &_fsm) + { + const auto &data = _fsm.dataPtr; + ignmsg << "The elevator is waiting to close door " << data->system->state + << std::endl; + + this->triggerEvent = [&_fsm] { _fsm.process_event(events::Timeout()); }; + data->system->StartDoorTimer(data->system->state, + std::bind(&WaitState::OnTimeout, this)); + } + + /// \brief Method that gets called upon timeout + private: void OnTimeout() + { + this->triggerEvent(); + } + + /// \brief Triggers the exit event + private: std::function triggerEvent; +}; + +/// \brief State at which the elevator is moving the cabin to the target floor. +struct ElevatorStateMachineDef::MoveCabinState : state +{ + /// \brief This state defers EnqueueNewTargetEvent events + public: using deferred_events = type_tuple; + + /// \brief Constructor + /// \param[in] _posEps Position tolerance when checking if the cabin has + /// reached the target floor level + /// \param[in] _velEps Velocity tolerance when checking if the cabin has + /// reached the target floor level + public: MoveCabinState(double _posEps = 15e-2, double _velEps = 1e-2) + : posEps(_posEps), velEps(_velEps) + { + } + + /// \brief Sends the command that moves the cabin joint to the target floor + /// and configures the monitor that checks the joint state + /// \param[in] _fsm State machine to which the state belongs + public: template + void on_enter(const Event &, FSM &_fsm) + { + const auto &data = _fsm.dataPtr; + int32_t floorTarget = data->targets.front(); + ignmsg << "The elevator is moving the cabin [ " << data->system->state + << " -> " << floorTarget << " ]" << std::endl; + + double jointTarget = data->system->cabinTargets[floorTarget]; + data->SendCmd(data->system->cabinJointCmdPub, jointTarget); + this->triggerEvent = [&_fsm] { + _fsm.process_event(events::CabinAtTarget()); + }; + data->system->SetCabinMonitor( + floorTarget, jointTarget, this->posEps, this->velEps, + std::bind(&MoveCabinState::OnJointTargetReached, this)); + } + + /// \brief Method that gets called when the cabin joint reaches its target + private: void OnJointTargetReached() + { + this->triggerEvent(); + } + + /// \brief Positiion tolerance + private: double posEps; + + /// \brief Velocity tolerance + private: double velEps; + + /// \brief Triggers the exit event + private: std::function triggerEvent; +}; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/utils/DoorTimer.cc b/src/systems/elevator/utils/DoorTimer.cc new file mode 100644 index 0000000000..960712f78a --- /dev/null +++ b/src/systems/elevator/utils/DoorTimer.cc @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include + +#include "DoorTimer.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +class DoorTimerPrivate +{ + /// \brief Constructor + /// \param[in] _waitDuration Duration + public: DoorTimerPrivate( + const std::chrono::steady_clock::duration &_waitDuration); + + /// \brief Wait duration + public: std::chrono::steady_clock::duration waitDuration; + + /// \brief Timeout time + public: std::chrono::steady_clock::duration timeoutTime; + + /// \brief Function that gets called upon timeout + public: std::function timeoutCallback; + + /// \brief Flag to indicate whether the doorway was blocked on the last update + /// time + public: bool wasDoorwayBlocked{false}; + + /// \brief Flag to indicate whether the timer is active + public: bool isActive{false}; +}; + +////////////////////////////////////////////////// +DoorTimerPrivate::DoorTimerPrivate( + const std::chrono::steady_clock::duration &_waitDuration) + : waitDuration(_waitDuration) +{ +} + +////////////////////////////////////////////////// +DoorTimer::DoorTimer(const std::chrono::steady_clock::duration &_waitDuration) + : dataPtr(std::make_unique(_waitDuration)) +{ +} + +////////////////////////////////////////////////// +DoorTimer::~DoorTimer() = default; + +////////////////////////////////////////////////// +void DoorTimer::Configure(const std::chrono::steady_clock::duration &_startTime, + const std::function &_timeoutCallback) +{ + this->dataPtr->isActive = true; + this->dataPtr->wasDoorwayBlocked = false; + this->dataPtr->timeoutTime = _startTime + this->dataPtr->waitDuration; + this->dataPtr->timeoutCallback = _timeoutCallback; +} + +////////////////////////////////////////////////// +void DoorTimer::Update(const UpdateInfo &_info, bool _isDoorwayBlocked) +{ + // Reset timeout time when doorway gets unblocked + if (!_isDoorwayBlocked && this->dataPtr->wasDoorwayBlocked) + this->dataPtr->timeoutTime = _info.simTime + this->dataPtr->waitDuration; + this->dataPtr->wasDoorwayBlocked = _isDoorwayBlocked; + + if (!this->dataPtr->isActive || _isDoorwayBlocked || + (_info.dt > std::chrono::steady_clock::duration::zero() && + _info.simTime < this->dataPtr->timeoutTime)) + return; + this->dataPtr->isActive = false; + this->dataPtr->timeoutCallback(); +} + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/utils/DoorTimer.hh b/src/systems/elevator/utils/DoorTimer.hh new file mode 100644 index 0000000000..bcfeb18a2f --- /dev/null +++ b/src/systems/elevator/utils/DoorTimer.hh @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_DOOR_TIMER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_DOOR_TIMER_HH_ + +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +// Data forward declaration +class DoorTimerPrivate; + +/// \brief Timer that's used to keep a door open. It has a configurable default +/// wait duration that when exceeded, it calls a function to let the state +/// machine know to transition to the next state. The timer also checks whether +/// the doorway is blocked, in which case it keeps the door open until whatever +/// blocks the doorway moves out of the way. +class DoorTimer +{ + /// \brief Constructor + /// \param[in] _waitDuration Duration + public: DoorTimer(const std::chrono::steady_clock::duration &_waitDuration); + + /// \brief Destructor + public: ~DoorTimer(); + + /// \brief Starts the timer and sets the timeout time based on the given + /// start time + /// \param[in] _startTime Start time + /// \param[in] _timeoutCallback Function to call upon timeout + public: void Configure(const std::chrono::steady_clock::duration &_startTime, + const std::function &_timeoutCallback); + + /// \brief Checks whether the timer has timed out + /// \param[in] _info Current simulation step info + /// \param[in] _isDoorwayBlocked Flag that indicates whether the doorway is + /// blocked + public: void Update(const UpdateInfo &_info, bool _isDoorwayBlocked); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; +}; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_SYSTEMS_DOOR_TIMER_HH_ diff --git a/src/systems/elevator/utils/JointMonitor.cc b/src/systems/elevator/utils/JointMonitor.cc new file mode 100644 index 0000000000..f2aa68a9a1 --- /dev/null +++ b/src/systems/elevator/utils/JointMonitor.cc @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#include + +#include "JointMonitor.hh" + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +class JointMonitorPrivate +{ + /// \brief Joint under monitoring + public: Entity joint; + + /// \brief Last joint target + public: double target; + + /// \brief Position tolerance + public: double posEps; + + /// \brief Velocity tolerance + public: double velEps; + + /// \brief Function that gets called when the joint reaches its target + public: std::function targetReachedCallback; + + /// \brief Flag to indicate whether the monitor is active + public: bool isActive{false}; +}; + +////////////////////////////////////////////////// +JointMonitor::JointMonitor() : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +JointMonitor::~JointMonitor() = default; + +////////////////////////////////////////////////// +void JointMonitor::Configure( + Entity _joint, double _target, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback) +{ + this->dataPtr->isActive = true; + this->dataPtr->joint = _joint; + this->dataPtr->target = _target; + this->dataPtr->posEps = _posEps; + this->dataPtr->velEps = _velEps; + this->dataPtr->targetReachedCallback = _jointTargetReachedCallback; +} + +////////////////////////////////////////////////// +void JointMonitor::Update(const EntityComponentManager &_ecm) +{ + if (!this->dataPtr->isActive) + return; + double pos = + _ecm.ComponentData(this->dataPtr->joint) + ->front(); + double vel = + _ecm.ComponentData(this->dataPtr->joint) + ->front(); + if (std::fabs(this->dataPtr->target - pos) > this->dataPtr->posEps || + vel > this->dataPtr->velEps) + return; + this->dataPtr->isActive = false; + this->dataPtr->targetReachedCallback(); +} + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/elevator/utils/JointMonitor.hh b/src/systems/elevator/utils/JointMonitor.hh new file mode 100644 index 0000000000..a42046b4ab --- /dev/null +++ b/src/systems/elevator/utils/JointMonitor.hh @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * \author Nick Lamprianidis + * \date January 2021 + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_JOINT_MONITOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_JOINT_MONITOR_HH_ + +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +// Data forward declaration +class JointMonitorPrivate; + +/// \brief Monitor that checks the state of a joint. When the joint reaches the +/// configured target, it calls a function to let the state machine know to +/// transition to the next state. +class JointMonitor +{ + /// \brief Constructor + public: JointMonitor(); + + /// \brief Destructor + public: ~JointMonitor(); + + /// \brief Starts monitoring of the position and velocity of the given joint + /// \param[in] _joint Joint to monitor + /// \param[in] _jointTarget Last joint target (command) + /// \param[in] _posEps Position tolerance + /// \param[in] _velEps Velocity tolerance + /// \param[in] _jointTargetReachedCallback Function to call when the joint + /// reaches its target + public: void Configure( + Entity _joint, double _target, double _posEps, double _velEps, + const std::function &_jointTargetReachedCallback); + + /// \brief Checks whether the position and velocity of the joint are within + /// the configured tolerances + /// \param[in] _ecm Entity component manager + public: void Update(const EntityComponentManager &_ecm); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; +}; + +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_SYSTEMS_JOINT_MONITOR_HH_ diff --git a/src/systems/elevator/vender/afsm/LICENSE b/src/systems/elevator/vender/afsm/LICENSE new file mode 100644 index 0000000000..6a3a57fb25 --- /dev/null +++ b/src/systems/elevator/vender/afsm/LICENSE @@ -0,0 +1,201 @@ + The Artistic License 2.0 + + Copyright (c) 2000-2006, The Perl Foundation. + + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +Preamble + +This license establishes the terms under which a given free software +Package may be copied, modified, distributed, and/or redistributed. +The intent is that the Copyright Holder maintains some artistic +control over the development of that Package while still keeping the +Package available as open source and free software. + +You are always permitted to make arrangements wholly outside of this +license directly with the Copyright Holder of a given Package. If the +terms of this license do not permit the full use that you propose to +make of the Package, you should contact the Copyright Holder and seek +a different licensing arrangement. + +Definitions + + "Copyright Holder" means the individual(s) or organization(s) + named in the copyright notice for the entire Package. + + "Contributor" means any party that has contributed code or other + material to the Package, in accordance with the Copyright Holder's + procedures. + + "You" and "your" means any person who would like to copy, + distribute, or modify the Package. + + "Package" means the collection of files distributed by the + Copyright Holder, and derivatives of that collection and/or of + those files. A given Package may consist of either the Standard + Version, or a Modified Version. + + "Distribute" means providing a copy of the Package or making it + accessible to anyone else, or in the case of a company or + organization, to others outside of your company or organization. + + "Distributor Fee" means any fee that you charge for Distributing + this Package or providing support for this Package to another + party. It does not mean licensing fees. + + "Standard Version" refers to the Package if it has not been + modified, or has been modified only in ways explicitly requested + by the Copyright Holder. + + "Modified Version" means the Package, if it has been changed, and + such changes were not explicitly requested by the Copyright + Holder. + + "Original License" means this Artistic License as Distributed with + the Standard Version of the Package, in its current version or as + it may be modified by The Perl Foundation in the future. + + "Source" form means the source code, documentation source, and + configuration files for the Package. + + "Compiled" form means the compiled bytecode, object code, binary, + or any other form resulting from mechanical transformation or + translation of the Source form. + + +Permission for Use and Modification Without Distribution + +(1) You are permitted to use the Standard Version and create and use +Modified Versions for any purpose without restriction, provided that +you do not Distribute the Modified Version. + + +Permissions for Redistribution of the Standard Version + +(2) You may Distribute verbatim copies of the Source form of the +Standard Version of this Package in any medium without restriction, +either gratis or for a Distributor Fee, provided that you duplicate +all of the original copyright notices and associated disclaimers. At +your discretion, such verbatim copies may or may not include a +Compiled form of the Package. + +(3) You may apply any bug fixes, portability changes, and other +modifications made available from the Copyright Holder. The resulting +Package will still be considered the Standard Version, and as such +will be subject to the Original License. + + +Distribution of Modified Versions of the Package as Source + +(4) You may Distribute your Modified Version as Source (either gratis +or for a Distributor Fee, and with or without a Compiled form of the +Modified Version) provided that you clearly document how it differs +from the Standard Version, including, but not limited to, documenting +any non-standard features, executables, or modules, and provided that +you do at least ONE of the following: + + (a) make the Modified Version available to the Copyright Holder + of the Standard Version, under the Original License, so that the + Copyright Holder may include your modifications in the Standard + Version. + + (b) ensure that installation of your Modified Version does not + prevent the user installing or running the Standard Version. In + addition, the Modified Version must bear a name that is different + from the name of the Standard Version. + + (c) allow anyone who receives a copy of the Modified Version to + make the Source form of the Modified Version available to others + under + + (i) the Original License or + + (ii) a license that permits the licensee to freely copy, + modify and redistribute the Modified Version using the same + licensing terms that apply to the copy that the licensee + received, and requires that the Source form of the Modified + Version, and of any works derived from it, be made freely + available in that license fees are prohibited but Distributor + Fees are allowed. + + +Distribution of Compiled Forms of the Standard Version +or Modified Versions without the Source + +(5) You may Distribute Compiled forms of the Standard Version without +the Source, provided that you include complete instructions on how to +get the Source of the Standard Version. Such instructions must be +valid at the time of your distribution. If these instructions, at any +time while you are carrying out such distribution, become invalid, you +must provide new instructions on demand or cease further distribution. +If you provide valid instructions or cease distribution within thirty +days after you become aware that the instructions are invalid, then +you do not forfeit any of your rights under this license. + +(6) You may Distribute a Modified Version in Compiled form without +the Source, provided that you comply with Section 4 with respect to +the Source of the Modified Version. + + +Aggregating or Linking the Package + +(7) You may aggregate the Package (either the Standard Version or +Modified Version) with other packages and Distribute the resulting +aggregation provided that you do not charge a licensing fee for the +Package. Distributor Fees are permitted, and licensing fees for other +components in the aggregation are permitted. The terms of this license +apply to the use and Distribution of the Standard or Modified Versions +as included in the aggregation. + +(8) You are permitted to link Modified and Standard Versions with +other works, to embed the Package in a larger work of your own, or to +build stand-alone binary or bytecode versions of applications that +include the Package, and Distribute the result without restriction, +provided the result does not expose a direct interface to the Package. + + +Items That are Not Considered Part of a Modified Version + +(9) Works (including, but not limited to, modules and scripts) that +merely extend or make use of the Package, do not, by themselves, cause +the Package to be a Modified Version. In addition, such works are not +considered parts of the Package itself, and are not subject to the +terms of this license. + + +General Provisions + +(10) Any use, modification, and distribution of the Standard or +Modified Versions is governed by this Artistic License. By using, +modifying or distributing the Package, you accept this license. Do not +use, modify, or distribute the Package, if you do not accept this +license. + +(11) If your Modified Version has been derived from a Modified +Version made by someone other than you, you are nevertheless required +to ensure that your Modified Version complies with the requirements of +this license. + +(12) This license does not grant you the right to use any trademark, +service mark, tradename, or logo of the Copyright Holder. + +(13) This license includes the non-exclusive, worldwide, +free-of-charge patent license to make, have made, use, offer to sell, +sell, import and otherwise transfer the Package with respect to any +patent claims licensable by the Copyright Holder that are necessarily +infringed by the Package. If you institute patent litigation +(including a cross-claim or counterclaim) against any party alleging +that the Package constitutes direct or contributory patent +infringement, then this Artistic License to you shall terminate on the +date that such litigation is filed. + +(14) Disclaimer of Warranty: +THE PACKAGE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS +IS' AND WITHOUT ANY EXPRESS OR IMPLIED WARRANTIES. THE IMPLIED +WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR +NON-INFRINGEMENT ARE DISCLAIMED TO THE EXTENT PERMITTED BY YOUR LOCAL +LAW. UNLESS REQUIRED BY LAW, NO COPYRIGHT HOLDER OR CONTRIBUTOR WILL +BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES ARISING IN ANY WAY OUT OF THE USE OF THE PACKAGE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/systems/elevator/vender/afsm/include/afsm/definition.hpp b/src/systems/elevator/vender/afsm/include/afsm/definition.hpp new file mode 100644 index 0000000000..7a0f2cb730 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/definition.hpp @@ -0,0 +1,721 @@ +/* + * definition.hpp + * + * Created on: 27 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DEFINITION_HPP_ +#define AFSM_DEFINITION_HPP_ + +#include +#include +#include +#include + +namespace afsm { +namespace def { + +namespace detail { + +template +struct source_state; + +template < typename SourceState, typename Event, typename TargetState, + typename Action, typename Guard> +struct source_state< transition > { + using type = SourceState; +}; + +template < typename Event, typename Action, typename Guard > +struct source_state< internal_transition< Event, Action, Guard > > { + using type = void; +}; + +template +struct target_state; + +template < typename SourceState, typename Event, typename TargetState, + typename Action, typename Guard> +struct target_state< transition > { + using type = TargetState; +}; + +template < typename Event, typename Action, typename Guard > +struct target_state< internal_transition< Event, Action, Guard > > { + using type = void; +}; + +template +struct event_type; + +template < typename SourceState, typename Event, typename TargetState, + typename Action, typename Guard> +struct event_type< transition > { + using type = Event; +}; +template < typename Event, typename Action, typename Guard > +struct event_type< internal_transition< Event, Action, Guard > > { + using type = Event; +}; + +template < typename SourceState, typename Event, typename Guard > +struct transition_key { + using source_state_type = SourceState; + using event_type = Event; + using guard_type = Guard; +}; + +} /* namespace detail */ + +template < typename SourceState, typename Event, typename TargetState, + typename Action, typename Guard > +struct transition { + using source_state_type = SourceState; + using target_state_type = TargetState; + using event_type = Event; + using action_type = Action; + using guard_type = Guard; + + using key_type = detail::transition_key; + + struct value_type { + using action_type = transition::action_type; + using target_state_type = transition::target_state_type; + }; +}; + +template < typename Event, typename Action, typename Guard > +struct internal_transition { + using event_type = Event; + using action_type = Action; + using guard_type = Guard; + + using key_type = detail::transition_key; + + struct value_type { + using action_type = internal_transition::action_type; + }; + + static_assert(!::std::is_same::value, + "Internal transition must have a trigger"); +}; + +template < typename Event > +struct handles_event { + template < typename Transition > + struct type : ::std::conditional< + ::std::is_same< typename Transition::event_type, Event >::value, + ::std::true_type, + ::std::false_type + >::type {}; +}; + +template < typename State > +struct originates_from { + template < typename Transition > + struct type : ::std::conditional< + ::std::is_same< typename Transition::source_state_type, State >::value, + ::std::true_type, + ::std::false_type + >::type {}; +}; + +template < typename ... T > +struct transition_table { + static_assert( + ::std::conditional< + (sizeof ... (T) > 0), + ::psst::meta::all_match< traits::is_transition, T ... >, + ::std::true_type + >::type::value, + "Transition table can contain only transition or internal_transition template instantiations" ); + using transitions = ::psst::meta::type_tuple; + using transition_map = ::psst::meta::type_map< + ::psst::meta::type_tuple, + ::psst::meta::type_tuple>; + using inner_states = typename ::psst::meta::unique< + typename detail::source_state::type ..., + typename detail::target_state::type ... + >::type; + using handled_events = typename ::psst::meta::unique< + typename T::event_type ... >::type; + + static constexpr ::std::size_t size = transition_map::size; + static constexpr ::std::size_t transition_count = transition_map::size; + static constexpr ::std::size_t inner_state_count = inner_states::size; + static constexpr ::std::size_t event_count = handled_events::size; + + static_assert( + ::std::conditional< + (inner_state_count > 0), + ::psst::meta::all_match< traits::is_state, inner_states >, + ::std::true_type + >::type::value, + "State types must derive from afsm::def::state"); + static_assert(transitions::size == transition_count, "Duplicate transition"); + // TODO Check for different guards for transitions from one state on one event +}; + +template < typename StateType, typename ... Tags > +struct state_def : tags::state, Tags... { + using state_type = StateType; + using base_state_type = state_def; + using internal_transitions = void; + using transitions = void; + using deferred_events = void; + using activity = void; + + template < typename Event, typename Action = none, typename Guard = none > + using in = internal_transition< Event, Action, Guard>; + template < typename ... T > + using transition_table = def::transition_table; + + using none = afsm::none; + template < typename Predicate > + using not_ = ::psst::meta::not_; + template < typename ... Predicates > + using and_ = ::psst::meta::and_; + template < typename ... Predicates > + using or_ = ::psst::meta::or_; + template < typename ... T > + using type_tuple = ::psst::meta::type_tuple; +}; + +template < typename StateType, typename ... Tags > +struct terminal_state : tags::state, Tags... { + using state_type = StateType; + using base_state_type = terminal_state; + using internal_transitions = void; + using transitions = void; + using deferred_events = void; + using activity = void; +}; + +template < typename StateType, typename Machine, typename ... Tags > +struct pushdown : tags::state, tags::pushdown, Tags... { + using state_type = StateType; + using base_state_type = pushdown; + using internal_transitions = void; + using transitions = void; + using deferred_events = void; + using activity = void; +}; + +template < typename StateType, typename Machine, typename ... Tags > +struct popup : tags::state, tags::popup, Tags... { + using state_type = StateType; + using base_state_type = popup; + using internal_transitions = void; + using transitions = void; + using deferred_events = void; + using activity = void; +}; + +template < typename StateMachine, typename ... Tags > +struct state_machine_def : state_def< StateMachine, Tags... >, tags::state_machine { + using state_machine_type = StateMachine; + using base_state_type = state_machine_def< state_machine_type, Tags... >; + using initial_state = void; + using internal_transitions = void; + using transitions = void; + using deferred_events = void; + using activity = void; + using orthogonal_regions = void; + + template + using tr = transition; + + using inner_states_definition = traits::inner_states_definitions>; + + template < typename T, typename ... TTags> + using state = typename inner_states_definition::template state; + template < typename T, typename ... TTags > + using terminal_state = typename inner_states_definition::template terminal_state; + template < typename T, typename ... TTags > + using state_machine = typename inner_states_definition::template state_machine; + template < typename T, typename M, typename ... TTags> + using push = typename inner_states_definition::template push; + template < typename T, typename M, typename ... TTags> + using pop = typename inner_states_definition::template pop; + + using none = afsm::none; + template < typename Predicate > + using not_ = ::psst::meta::not_; + template < typename ... Predicates > + using and_ = ::psst::meta::and_; + template < typename ... Predicates > + using or_ = ::psst::meta::or_; + template < typename ... T > + using type_tuple = ::psst::meta::type_tuple; +}; + +namespace detail { + +template < typename T > +struct has_inner_states : ::std::false_type {}; +template < typename ... T > +struct has_inner_states< transition_table > + : ::std::integral_constant::inner_state_count > 0)> {}; + +template < typename T > +struct inner_states { + using type = ::psst::meta::type_tuple<>; +}; + +template < typename ... T > +struct inner_states< transition_table > { + using type = typename transition_table::inner_states; +}; + +template < typename ... T > +struct inner_states< ::psst::meta::type_tuple > { + using type = ::psst::meta::type_tuple; +}; + +template < typename T > +struct has_transitions : ::std::false_type {}; +template < typename ... T > +struct has_transitions< transition_table > + : ::std::conditional< + (transition_table::transition_count > 0), + ::std::true_type, + ::std::false_type + >::type{}; + +template < typename T > +struct handled_events + : std::conditional< + traits::is_state_machine::value, + handled_events>, + handled_events> + >::type {}; + +template < typename ... T > +struct handled_events< transition_table > { + using type = typename transition_table::handled_events; +}; + +template <> +struct handled_events { + using type = ::psst::meta::type_tuple<>; +}; + +template < typename T > +struct handled_events< state > { + using type = typename handled_events< + typename T::internal_transitions >::type; +}; + +template < typename T > +struct handled_events< state_machine > { + using type = typename ::psst::meta::unique< + typename handled_events< typename T::internal_transitions >::type, + typename handled_events< typename T::transitions >::type + >::type; +}; + +/** + * Events handled by a set of states + */ +template < typename ... T > +struct handled_events< ::psst::meta::type_tuple > { + using type = typename ::psst::meta::unique< + typename handled_events::type ... + >::type; +}; + +template < typename T > +struct recursive_handled_events + : ::std::conditional< + traits::is_state_machine::value, + recursive_handled_events< state_machine >, + handled_events< state > + >::type {}; + +template < typename ... T > +struct recursive_handled_events< transition_table > { + using type = typename ::psst::meta::unique< + typename transition_table::handled_events, + typename recursive_handled_events< + typename transition_table::inner_states >::type + >::type; +}; + +template <> +struct recursive_handled_events { + using type = ::psst::meta::type_tuple<>; +}; + +template < typename T > +struct recursive_handled_events< state_machine > { + using type = + typename ::psst::meta::unique< + typename ::psst::meta::unique< + typename handled_events< typename T::internal_transitions >::type, + typename recursive_handled_events< typename T::transitions >::type + >::type, + typename recursive_handled_events< typename T::orthogonal_regions >::type + >::type; +}; + +template < typename T, typename ... Y > +struct recursive_handled_events< ::psst::meta::type_tuple > { + using type = typename ::psst::meta::unique< + typename recursive_handled_events::type, + typename recursive_handled_events< ::psst::meta::type_tuple>::type + >::type; +}; + +template < typename T > +struct recursive_handled_events< ::psst::meta::type_tuple > + : recursive_handled_events {}; + +template <> +struct recursive_handled_events< ::psst::meta::type_tuple<> > { + using type = ::psst::meta::type_tuple<>; +}; + +template < typename T > +struct has_default_transitions; + +template < typename ... T > +struct has_default_transitions< ::psst::meta::type_tuple > + : ::psst::meta::contains< none, ::psst::meta::type_tuple > {}; + +template < typename T > +struct is_default_transition + : ::std::conditional< + ::std::is_same< typename T::event_type, none >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +template < typename T > +struct is_unguarded_transition + : ::std::conditional< + ::std::is_same< typename T::guard_type, none >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +template < typename T > +struct is_default_unguarded_transition + : ::std::conditional< + is_default_transition::value && is_unguarded_transition::value, + ::std::true_type, + ::std::false_type + >::type {}; + +} /* namespace detail */ + +//---------------------------------------------------------------------------- +// Metafunction to check if a machine contains a given state +//---------------------------------------------------------------------------- +template < typename T, typename SubState > +struct contains_substate; + +namespace detail { + +template < typename T, typename SubState > +struct contains_recursively : + ::std::conditional< + ::std::is_same::value, + ::std::true_type, + typename def::contains_substate::type + >::type {}; + +template < typename SubState > +struct contains_predicate { + template < typename T > + using type = contains_recursively; +}; + +template < typename T, typename SubState, bool IsMachine > +struct contains_substate : ::std::false_type {}; + +template < typename T, typename SubState > +struct contains_substate< T, SubState, true > + : ::std::conditional< + ::psst::meta::any_match< + contains_predicate::template type, + typename ::psst::meta::unique< + typename inner_states< typename T::transitions >::type, + typename inner_states< typename T::orthogonal_regions >::type + >::type + >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +} /* namespace detail */ + +template < typename T, typename SubState > +struct contains_substate : + detail::contains_substate::value> {}; + +template < typename SubState > +struct contains_substate_predicate { + template < typename T > + using type = contains_substate; +}; + +//---------------------------------------------------------------------------- +// Metafunction to calculate a path from a machine to a substate +//---------------------------------------------------------------------------- +template < typename Machine, typename State > +struct state_path; + +namespace detail { + +template < typename T, typename U > +struct state_state_path { + using type = ::psst::meta::type_tuple<>; +}; + +template < typename T > +struct state_state_path { + using type = ::psst::meta::type_tuple; +}; + +template < typename Machine, typename State, bool IsMachine > +struct state_path : state_state_path {}; + +template < typename Machine, typename State, bool Contains > +struct machine_state_path { + using type = ::psst::meta::type_tuple<>; +}; + +template < typename Machine, typename State > +struct machine_state_path { + using type = typename ::psst::meta::combine< + typename Machine::state_machine_type, + typename def::state_path< + typename ::psst::meta::front< + typename ::psst::meta::find_if< + contains_predicate::template type, + typename ::psst::meta::unique< + typename inner_states< typename Machine::transitions >::type, + typename inner_states< typename Machine::orthogonal_regions >::type + >::type + >::type + >::type, + State + >::type + >::type; +}; + +template < typename Machine, typename State > +struct state_path + : ::std::conditional< + ::std::is_same::value, + state_state_path, + machine_state_path::value> + >::type {}; + +} /* namespace detail */ + +template < typename Machine, typename State > +struct state_path + : detail::state_path::value> {}; + +//---------------------------------------------------------------------------- +// Metafunction to check if a machine contains pushdown states +//---------------------------------------------------------------------------- +template < typename T > +struct contains_pushdowns; + +namespace detail { + +template < typename T > +struct contains_pushdowns_recursively : + ::std::conditional< + traits::is_pushdown::value, + ::std::true_type, + typename def::contains_pushdowns::type + >::type {}; + +template < typename T, bool IsMachine > +struct containts_pushdowns : ::std::false_type {}; + +template < typename T > +struct containts_pushdowns + : ::std::conditional< + ::psst::meta::any_match< + contains_pushdowns_recursively, + typename ::psst::meta::unique< + typename inner_states< typename T::transitions >::type, + typename inner_states< typename T::orthogonal_regions >::type + >::type + >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +} /* namespace detail */ + +template < typename T > +struct contains_pushdowns + : detail::containts_pushdowns::value>{}; + +//---------------------------------------------------------------------------- +// Metafunction to check if a machine contains popup states +//---------------------------------------------------------------------------- +template < typename T > +struct contains_popups; + +namespace detail { + +template < typename T > +struct contains_popups_recursively : + ::std::conditional< + traits::is_popup::value, + ::std::true_type, + typename def::contains_popups::type + >::type {}; + +template < typename T, bool IsMachine > +struct containts_popups : ::std::false_type {}; + +template < typename T > +struct containts_popups + : ::std::conditional< + ::psst::meta::any_match< + contains_popups_recursively, + typename ::psst::meta::unique< + typename inner_states< typename T::transitions >::type, + typename inner_states< typename T::orthogonal_regions >::type + >::type + >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +} /* namespace detail */ + +template < typename T > +struct contains_popups + : detail::containts_popups::value>{}; + +//---------------------------------------------------------------------------- +// Metafunction to check if a machine is pushed by a substate +//---------------------------------------------------------------------------- +template < typename T > +struct is_pushed; +template < typename T, typename Machine> +struct state_pushes; + +namespace detail { + +template < typename Machine > +struct pushes_predicate { + template < typename T > + using type = def::state_pushes; +}; + + +template < typename T, typename Machine, bool IsMachine > +struct state_pushes + : ::std::integral_constant::value> {}; +template < typename T, typename Machine > +struct state_pushes + : ::std::integral_constant::template type, + typename ::psst::meta::unique< + typename inner_states< typename T::transitions >::type, + typename inner_states< typename T::orthogonal_regions >::type + >::type + >::value + >{}; + +template < typename Machine, bool IsMachine > +struct is_pushed : ::std::false_type {}; + +template < typename Machine > +struct is_pushed + : ::std::integral_constant::template type, + typename ::psst::meta::unique< + typename inner_states< typename Machine::transitions >::type, + typename inner_states< typename Machine::orthogonal_regions >::type + >::type + >::value + > {}; + +} /* namespace detail */ + +template < typename T > +struct is_pushed : detail::is_pushed::value> {}; +template < typename T, typename Machine > +struct state_pushes + : detail::state_pushes::value> {}; + +//---------------------------------------------------------------------------- +// Metafunction to check if a machine is popped by a substate +//---------------------------------------------------------------------------- +template < typename T > +struct is_popped; +template < typename T, typename Machine> +struct state_pops; + +namespace detail { + +template < typename Machine > +struct pops_predicate { + template < typename T > + using type = def::state_pops; +}; + + +template < typename T, typename Machine, bool IsMachine > +struct state_pops + : ::std::integral_constant::value> {}; +template < typename T, typename Machine > +struct state_pops + : ::std::integral_constant::template type, + typename ::psst::meta::unique< + typename inner_states< typename T::transitions >::type, + typename inner_states< typename T::orthogonal_regions >::type + >::type + >::value + >{}; + +template < typename Machine, bool IsMachine > +struct is_popped : ::std::false_type {}; + +template < typename Machine > +struct is_popped + : ::std::integral_constant::template type, + typename ::psst::meta::unique< + typename inner_states< typename Machine::transitions >::type, + typename inner_states< typename Machine::orthogonal_regions >::type + >::type + >::value + > {}; + +} /* namespace detail */ + +template < typename T > +struct is_popped : detail::is_popped::value> {}; +template < typename T, typename Machine > +struct state_pops + : detail::state_pops::value> {}; + +template < typename T > +struct has_pushdown_stack + : ::std::integral_constant::value && is_popped::value> {}; + +} /* namespace def */ +} /* namespace afsm */ + + +#endif /* AFSM_DEFINITION_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/definition_fwd.hpp b/src/systems/elevator/vender/afsm/include/afsm/definition_fwd.hpp new file mode 100644 index 0000000000..3ea9a8e4e9 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/definition_fwd.hpp @@ -0,0 +1,51 @@ +/* + * definition_fwd.hpp + * + * Created on: 1 июня 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DEFINITION_FWD_HPP_ +#define AFSM_DEFINITION_FWD_HPP_ + +namespace afsm { +namespace def { + +template < typename T, typename ... Tags > +struct state_def; +template < typename StateType, typename ... Tags > +using state = state_def< StateType, Tags... >; + +template < typename T, typename ... Tags > +struct terminal_state; + +template < typename T, typename Machine, typename ... Tags > +struct pushdown; +template < typename T, typename Machine, typename ... Tags > +struct popup; + +template < typename T, typename ... Tags > +struct state_machine_def; +template < typename T, typename ... Tags > +using state_machine = state_machine_def; + +template < typename SourceState, typename Event, typename TargetState, + typename Action = none, typename Guard = none > +struct transition; +template < typename Event, typename Action = none, typename Guard = none > +struct internal_transition; +template < typename ... T > +struct transition_table; + +} /* namespace def */ +namespace detail { + +template +struct pushdown_state; + +template +struct popup_state; +} /* namespace detail */ +} /* namespace afsm */ + +#endif /* AFSM_DEFINITION_FWD_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/actions.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/actions.hpp new file mode 100644 index 0000000000..d64d08b8a4 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/actions.hpp @@ -0,0 +1,503 @@ +/* + * actions.hpp + * + * Created on: 28 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_ACTIONS_HPP_ +#define AFSM_DETAIL_ACTIONS_HPP_ + +#include +#include +#include + +namespace afsm { + +namespace detail { + +template < ::std::size_t StateIndex > +struct set_enclosing_fsm { + static constexpr ::std::size_t state_index = StateIndex; + + template < typename FSM, typename ... T > + static void + set(FSM& fsm, ::std::tuple& states) + { + set_enclosing_fsm::set(fsm, states); + ::std::get(states).enclosing_fsm(fsm); + } +}; + +template <> +struct set_enclosing_fsm<0> { + static constexpr ::std::size_t state_index = 0; + + template < typename FSM, typename ... T > + static void + set(FSM& fsm, ::std::tuple& states) + { + ::std::get(states).enclosing_fsm(fsm); + } +}; + +} /* namespace detail */ + +namespace actions { + +enum class event_process_result { + refuse, + defer, + process_in_state, /**< Process with in-state transition */ + process, +}; + +/** + * The event was accepted and either processed or deferred for later processing. + * @param res + * @return + */ +inline bool +ok(event_process_result res) +{ + return res != event_process_result::refuse; +} + +/** + * The event was processed either with or without state transition. + * @param res + * @return + */ +inline bool +done(event_process_result res) +{ + return res == event_process_result::process || + res == event_process_result::process_in_state; +} + +namespace detail { + +template +struct action_long_signature { +private: + static FSM& fsm; + static Event& event; + static SourceState& source; + static TargetState& target; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval()(::std::move(event), fsm, source, target) ) const* ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template +struct action_short_signature { +private: + static FSM& fsm; + static Event& event; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval()(::std::move(event), fsm) ) const* ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template +struct handles_reject { +private: + static FSM& fsm; + static Event& event; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval().reject_event(::std::move(event), fsm) ) const * ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template +struct handles_event + : ::std::conditional< + ::std::is_same< typename Transition::event_type, Event >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +template < typename Guard, typename FSM, typename State, typename Event > +struct guard_wants_event + : ::std::integral_constant::value> {}; + +template < typename Guard, typename FSM, typename State, typename Event > +struct guard_wants_event< ::psst::meta::not_, FSM, State, Event > + : ::std::integral_constant::value> {}; + +template < bool WantEvent, typename FSM, typename State, typename Event, typename Guard > +struct guard_event_check { + bool + operator()(FSM const& fsm, State const& state, Event const&) const + { return Guard{}(fsm, state); } +}; + +template < typename FSM, typename State, typename Event, typename Guard > +struct guard_event_check< true, FSM, State, Event, Guard > { + bool + operator()(FSM const& fsm, State const& state, Event const& event) const + { return Guard{}(fsm, state, event); } +}; + +template < typename FSM, typename State, typename Event, typename Guard > +struct guard_check : ::std::conditional< + guard_wants_event< Guard, FSM, State, Event >::value, + guard_event_check, + typename ::std::conditional< + ::psst::meta::is_callable< Guard, FSM const&, State const& >::value, + guard_event_check, + guard_check // TODO Replace with concept assert + >::type + >::type {}; + +template < typename FSM, typename State, typename Event, typename ... Guards > +struct guard_check< FSM, State, Event, ::psst::meta::and_ > { + using check_function = ::psst::meta::and_< guard_check< FSM, State, Event, Guards >... >; + bool + operator()(FSM const& fsm, State const& state, Event const& event) const + { + return check_function{}(fsm, state, event); + } +}; + +template < typename FSM, typename State, typename Event, typename ... Guards > +struct guard_check< FSM, State, Event, ::psst::meta::or_ > { + using check_function = ::psst::meta::or_< guard_check< FSM, State, Event, Guards >... >; + bool + operator()(FSM const& fsm, State const& state, Event const& event) const + { + return check_function{}(fsm, state, event); + } +}; + +template < typename FSM, typename State, typename Event > +struct guard_check< FSM, State, Event, none > { + constexpr bool + operator()(FSM const&, State const&, Event const&) const + { return true; } +}; + +template < typename Action, typename Event, typename FSM, + typename SourceState, typename TargetState, bool LongSignature > +struct action_invocation_impl { + void + operator()(Event&& event, FSM& fsm, SourceState& source, TargetState& target) const + { + static_assert(action_long_signature< Action, Event, + FSM, SourceState, TargetState >::value, + "Action is not callable for this transition"); + Action{}(::std::forward(event), fsm, source, target); + } +}; + +template < typename Action, typename Event, typename FSM, + typename SourceState, typename TargetState > +struct action_invocation_impl { + void + operator()(Event&& event, FSM& fsm, SourceState&, TargetState&) const + { + static_assert(action_short_signature< Action, Event, + FSM >::value, + "Action is not callable for this transition"); + Action{}(::std::forward(event), fsm); + } +}; + + +template < typename Action, typename FSM, + typename SourceState, typename TargetState > +struct action_invocation { + template < typename Event > + void + operator()(Event&& event, FSM& fsm, SourceState& source, TargetState& target) const + { + using invocation_type = action_invocation_impl< + Action, Event, FSM, + SourceState, TargetState, + action_long_signature::value>; + invocation_type{}(::std::forward(event), fsm, source, target); + } +}; + +template < typename FSM, + typename SourceState, typename TargetState > +struct action_invocation< none, FSM, SourceState, TargetState > { + template < typename Event > + void + operator()(Event&&, FSM&, SourceState&, TargetState&) const + {} +}; + +template < typename Action, typename Guard, typename FSM, + typename SourceState, typename TargetState > +struct guarded_action_invocation { + using invocation_type = action_invocation< Action, FSM, SourceState, TargetState >; + + template < typename Event > + event_process_result + operator()(Event&& event, FSM& fsm, SourceState& source, TargetState& target) const + { + using guard_type = guard_check< FSM, SourceState, Event, Guard >; + if (guard_type{}(fsm, source, ::std::forward(event))) { + invocation_type{}(::std::forward(event), fsm, source, target); + return event_process_result::process; + } + return event_process_result::refuse; + } +}; + +template < ::std::size_t N, typename FSM, typename State, typename Transitions > +struct nth_inner_invocation { + static_assert(Transitions::size > N, "Transitions list is too small"); + using transition = typename Transitions::template type; + using action_type = typename transition::action_type; + using guard_type = typename transition::guard_type; + using invocation_type = guarded_action_invocation< + action_type, guard_type, FSM, State, State >; + using previous_action = nth_inner_invocation; + + template < typename Event > + event_process_result + operator()(Event&& event, FSM& fsm, State& state) const + { + auto res = previous_action{}(::std::forward(event), fsm, state); + if (res == event_process_result::refuse) + return invocation_type{}(::std::forward(event), fsm, state, state); + return res; + } +}; + +template < typename FSM, typename State, typename Transitions > +struct nth_inner_invocation<0, FSM, State, Transitions> { + static_assert(Transitions::size > 0, "Transitions list is empty"); + using transition = typename Transitions::template type<0>; + using action_type = typename transition::action_type; + using guard_type = typename transition::guard_type; + using invocation_type = guarded_action_invocation< + action_type, guard_type, FSM, State, State >; + + template < typename Event > + event_process_result + operator()(Event&& event, FSM& fsm, State& state) const + { + return invocation_type{}(::std::forward(event), fsm, state, state); + } +}; + +struct no_in_state_invocation { + template < typename FSM, typename State, typename Event > + event_process_result + operator()(Event&&, FSM&, State&) const + { + static_assert(::std::is_same::value, ""); + return event_process_result::refuse; + } +}; + +template < typename FSM, typename State, typename Transition > +struct unconditional_in_state_invocation { + using action_type = typename Transition::action_type; + using guard_type = typename Transition::guard_type; + using invocation_type = guarded_action_invocation< + action_type, guard_type, FSM, State, State >; + + template < typename Event > + event_process_result + operator()(Event&& event, FSM& fsm, State& state) const + { + return invocation_type{}(::std::forward(event), fsm, state, state); + } +}; + +template < typename FSM, typename State, typename Transitions > +struct conditional_in_state_invocation { + static constexpr ::std::size_t size = Transitions::size; + using invocation_type = nth_inner_invocation< size - 1, FSM, State, Transitions>; + + template < typename Event > + event_process_result + operator()(Event&& event, FSM& fsm, State& state) const + { + return invocation_type{}(::std::forward(event), fsm, state); + } +}; + +template < bool hasActions, typename State, typename Event > +struct is_in_state_action { + using state_type = State; + using event_type = Event; + using transitions = typename state_type::internal_transitions; + using event_handlers = typename ::psst::meta::find_if< + def::handles_event::template type, + typename transitions::transitions >::type; + + static constexpr bool value = event_handlers::size > 0; +}; + +template < typename State, typename Event > +struct is_in_state_action { + static constexpr bool value = false; +}; + +template < bool hasActions, typename FSM, typename State, typename Event > +struct in_state_action_invocation { + using fsm_type = FSM; + using state_type = State; + using event_type = Event; + + using transitions = typename state_type::internal_transitions; + static_assert( !::std::is_same::value, + "State doesn't have internal transitions table" ); + + using event_handlers = typename ::psst::meta::find_if< + def::handles_event::template type, + typename transitions::transitions >::type; + static_assert( event_handlers::size > 0, "State doesn't handle event" ); + using handler_type = typename ::std::conditional< + event_handlers::size == 1, + detail::unconditional_in_state_invocation< + fsm_type, state_type, typename event_handlers::template type<0>>, + detail::conditional_in_state_invocation< fsm_type, state_type, event_handlers> + >::type; + + template < typename Evt > + event_process_result + operator()(Evt&& event, fsm_type& fsm, state_type& state) const + { + auto res = handler_type{}(::std::forward(event), fsm, state); + if (res == event_process_result::process) { + return event_process_result::process_in_state; + } + return res; + } +}; + +template < typename FSM, typename State, typename Event > +struct in_state_action_invocation { + using fsm_type = FSM; + using state_type = State; + using event_type = Event; + + template < typename Evt > + event_process_result + operator()(Evt&&, fsm_type&, state_type&) const + { + return event_process_result::refuse; + } +}; + +} /* namespace detail */ + +template < typename State, typename Event > +struct is_in_state_action : detail::is_in_state_action < + !::std::is_same::value && + ::psst::meta::contains::value, + State, Event >{}; + +template < typename FSM, typename State, typename Event > +struct in_state_action_invocation : + detail::in_state_action_invocation< + !::std::is_same::value && + ::psst::meta::contains::value, + FSM, State, Event > { +}; + +template < typename FSM, typename State, typename Event > +event_process_result +handle_in_state_event(Event&& event, FSM& fsm, State& state) +{ + using decayed_event = typename ::std::decay::type; + return in_state_action_invocation< FSM, State, decayed_event >{} + (::std::forward(event), fsm, state); +} + +namespace detail { + +template < ::std::size_t StateIndex > +struct process_event_handler { + static constexpr ::std::size_t state_index = StateIndex; + template < typename StateTuple, typename Event > + event_process_result + operator()(StateTuple& states, Event&& event) const + { + return ::std::get(states).process_event(::std::forward(event)); + } +}; + +template < typename Indexes > +struct handlers_tuple; + +template < ::std::size_t ... Indexes > +struct handlers_tuple < ::psst::meta::indexes_tuple > { + using type = ::std::tuple< process_event_handler... >; + static constexpr ::std::size_t size = sizeof ... (Indexes); +}; + +template < typename T > +class inner_dispatch_table; + +template < typename ... T > +class inner_dispatch_table< ::std::tuple > { +public: + static constexpr ::std::size_t size = sizeof ... (T); + using states_tuple = ::std::tuple; + using indexes_tuple = typename ::psst::meta::index_builder< size >::type; + using dispatch_tuple = typename handlers_tuple::type; + template < typename Event > + using invocation_table = ::std::array< + ::std::function< event_process_result(states_tuple&, Event&&) >, size >; +public: + explicit + inner_dispatch_table() {} + + template < typename Event > + static event_process_result + process_event(states_tuple& states, ::std::size_t current_state, Event&& event) + { + //using event_type = typename ::std::decay::type; + if (current_state >= size) + throw ::std::logic_error{ "Invalid current state index" }; + auto inv_table = state_table< Event >(indexes_tuple{}); + return inv_table[current_state](states, ::std::forward(event)); + } +private: + template < typename Event, ::std::size_t ... Indexes > + static invocation_table const& + state_table( ::psst::meta::indexes_tuple< Indexes... > const& ) + { + static invocation_table _table {{ process_event_handler{}... }}; + return _table; + } +}; + +} /* namespace detail */ + +} /* namespace actions */ +} /* namespace afsm */ + +#endif /* AFSM_DETAIL_ACTIONS_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/base_states.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/base_states.hpp new file mode 100644 index 0000000000..11563fce0b --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/base_states.hpp @@ -0,0 +1,991 @@ +/* + * base_states.hpp + * + * Created on: 28 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_BASE_STATES_HPP_ +#define AFSM_DETAIL_BASE_STATES_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace afsm { +namespace detail { + +template < actions::event_process_result R > +using process_type = ::std::integral_constant< actions::event_process_result, R >; + +template > +struct event_process_selector + : ::std::conditional< + ::psst::meta::contains::type, HandledEvents>::value, + process_type, + typename ::std::conditional< + ::psst::meta::contains::type, DeferredEvents>::value, + process_type, + process_type + >::type + >::type{}; + +enum class state_containment { + none, + self, + immediate, + substate +}; + +template < state_containment C > +using containment_type = ::std::integral_constant; + +using state_containment_none = containment_type< state_containment::none >; +using state_containment_self = containment_type< state_containment::self >; +using state_containment_immediate = containment_type< state_containment::immediate >; +using state_containment_substate = containment_type< state_containment::substate >; + +template < typename StateDef, typename MachineDef, typename InnerStates > +struct state_containment_type : + ::std::conditional< + ::std::is_same< MachineDef, StateDef >::value, + state_containment_self, + typename ::std::conditional < + ::psst::meta::contains::value, + state_containment_immediate, + typename ::std::conditional< + ::psst::meta::any_match< + def::contains_substate_predicate::template type, InnerStates >::value, + state_containment_substate, + state_containment_none + >::type + >::type + >::type {}; + +template < typename T, bool isTerminal > +struct state_base_impl : T { + using state_definition_type = T; + using state_type = state_base_impl; + using internal_transitions = typename state_definition_type::internal_transitions; + + static_assert(def::traits::is_state::value, + "Front state can be created only with a descendant of afsm::def::state"); + static_assert(!def::detail::has_inner_states< internal_transitions >::value, + "Internal transition table cannot have transitions between other states"); + using handled_events = + typename def::detail::handled_events::type; + using internal_events = + typename def::detail::handled_events::type; + static_assert(def::traits::is_state_machine::value + || !def::detail::has_default_transitions< handled_events >::value, + "Internal transition cannot be a default transition"); + using deferred_events = + typename ::std::conditional< + ::std::is_same::value, + ::psst::meta::type_tuple<>, + typename ::psst::meta::unique< typename T::deferred_events >::type + >::type; + + state_base_impl() : state_definition_type{} {} + state_base_impl(state_base_impl const&) = default; + state_base_impl(state_base_impl&&) = default; + + state_base_impl& + operator = (state_base_impl const&) = delete; + state_base_impl& + operator = (state_base_impl&&) = delete; + + void + swap(state_base_impl& rhs) noexcept + { + using ::std::swap; + swap(static_cast(*this), static_cast(rhs)); + } + template < typename Event, typename FSM > + void + state_enter(Event&&, FSM&) {} + template < typename Event, typename FSM > + void + state_exit(Event&&, FSM&) {} + + event_set const& + current_handled_events() const + { return static_handled_events(); } + event_set const& + current_deferrable_events() const + { return static_deferrable_events(); } + + static event_set const& + static_handled_events() + { + static event_set evts_ = make_event_set(handled_events{}); + return evts_; + } + static event_set const& + internal_handled_events() + { + static event_set evts_ = make_event_set(typename def::detail::handled_events< internal_transitions >::type{}); + return evts_; + } + + static event_set const& + static_deferrable_events() + { + static event_set evts_ = make_event_set(deferred_events{}); + return evts_; + } +protected: + template< typename ... Args > + state_base_impl(Args&& ... args) + : state_definition_type(::std::forward(args)...) {} +}; + +template < typename T > +struct state_base_impl : T { + using state_definition_type = T; + using state_type = state_base_impl; + using internal_transitions = typename state_definition_type::internal_transitions; + + static_assert(def::traits::is_state::value, + "Front state can be created only with a descendant of afsm::def::state"); + static_assert(::std::is_same< + typename state_definition_type::internal_transitions, void >::value, + "Terminal state must not define transitions"); + static_assert(::std::is_same< + typename state_definition_type::deferred_events, void >::value, + "Terminal state must not define deferred events"); + + using handled_events = ::psst::meta::type_tuple<>; + using internal_events = ::psst::meta::type_tuple<>; + using deferred_events = ::psst::meta::type_tuple<>; + + state_base_impl() : state_definition_type{} {} + state_base_impl(state_base_impl const&) = default; + state_base_impl(state_base_impl&&) = default; + + state_base_impl& + operator = (state_base_impl const&) = delete; + state_base_impl& + operator = (state_base_impl&&) = delete; + + void + swap(state_base_impl& rhs) noexcept + { + using ::std::swap; + swap(static_cast(*this), static_cast(rhs)); + } + + template < typename Event, typename FSM > + void + state_enter(Event&&, FSM&) {} + template < typename Event, typename FSM > + void + state_exit(Event&&, FSM&) {} + + event_set const& + current_handled_events() const + { return static_handled_events(); } + event_set const& + current_deferrable_events() const + { return static_deferrable_events(); } + + static event_set const& + static_handled_events() + { + static event_set evts_{}; + return evts_; + } + static event_set const& + internal_handled_events() + { + static event_set evts_{}; + return evts_; + } + static event_set const& + static_deferrable_events() + { + static event_set evts_{}; + return evts_; + } +protected: + template< typename ... Args > + state_base_impl(Args&& ... args) + : state_definition_type(::std::forward(args)...) {} +}; + +template < typename T > +struct pushdown_state : state_base_impl { + using pushdown_machine_type = typename T::pushdown_machine_type; + + template < typename Event, typename FSM > + void + state_enter(Event&& event, FSM& fsm) + { + root_machine(fsm).template get_state< pushdown_machine_type >().pushdown(::std::forward(event)); + } +}; + +template < typename T > +struct popup_state : state_base_impl { // TODO Check if the state is terminal by tags + using pushdown_machine_type = typename T::pushdown_machine_type; + + template < typename Event, typename FSM > + void + state_enter(Event&& event, FSM& fsm) + { + root_machine(fsm).template get_state< pushdown_machine_type >().popup(::std::forward(event)); + } +}; + +template < typename T > +class state_base : public ::std::conditional< + def::traits::is_pushdown::value, + pushdown_state, + typename ::std::conditional< + def::traits::is_popup::value, + popup_state, + state_base_impl::value> + >::type + >::type { +public: + using state_definition_type = T; + using state_type = state_base; + using internal_transitions = typename state_definition_type::internal_transitions; + using base_impl_type = typename ::std::conditional< + def::traits::is_pushdown::value, + pushdown_state, + typename ::std::conditional< + def::traits::is_popup::value, + popup_state, + state_base_impl::value> + >::type + >::type; +public: + state_base() : base_impl_type{} {} + state_base(state_base const&) = default; + state_base(state_base&&) = default; + + state_base& + operator = (state_base const&) = delete; + state_base& + operator = (state_base&&) = delete; + + void + swap(state_base& rhs) noexcept + { + using ::std::swap; + static_cast(*this).swap(rhs); + } + + template < typename StateDef > + bool + is_in_state() const + { + return ::std::is_same::value; + } +protected: + template< typename ... Args > + state_base(Args&& ... args) + : base_impl_type(::std::forward(args)...) {} +}; + +template < typename T, typename Mutex, typename FrontMachine > +class state_machine_base_impl : public state_base, + public transitions::transition_container::type> { +public: + using state_machine_definition_type = T; + using state_type = state_base; + using machine_type = state_machine_base_impl; + using front_machine_type = FrontMachine; + static_assert(def::traits::is_state_machine::value, + "Front state machine can be created only with a descendant of afsm::def::state_machine"); + using transitions = typename state_machine_definition_type::transitions; + using handled_events = + typename def::detail::recursive_handled_events::type; + static_assert(def::traits::is_state< + typename state_machine_definition_type::initial_state >::value, + "State machine definition must specify an initial state"); + using initial_state = typename state_machine_definition_type::initial_state; + using inner_states_def = + typename def::detail::inner_states< transitions >::type; + + static constexpr ::std::size_t initial_state_index = + ::psst::meta::index_of::value; + static constexpr ::std::size_t inner_state_count = inner_states_def::size; + + using state_indexes = typename ::psst::meta::index_builder::type; + + using mutex_type = Mutex; + using size_type = typename detail::size_type::type; + using has_pushdowns = def::has_pushdown_stack; + using transition_container = afsm::transitions::transition_container_selector< + front_machine_type, state_machine_definition_type, size_type, has_pushdowns::value>; + using transition_tuple = typename transition_container::type; + using inner_states_tuple = typename transition_tuple::inner_states_tuple; + + using container_base = + afsm::transitions::transition_container< + front_machine_type, state_machine_definition_type, size_type>; + + template < typename State > + using substate_type = typename detail::substate_type::type; + template < typename State > + using contains_substate = def::contains_substate; +public: + state_machine_base_impl(front_machine_type* fsm) + : state_type{}, + container_base{fsm} + {} + + state_machine_base_impl(front_machine_type* fsm, state_machine_base_impl const& rhs) + : state_type{static_cast(rhs)}, + container_base{fsm, rhs} + {} + state_machine_base_impl(front_machine_type* fsm, state_machine_base_impl&& rhs) + : state_type{static_cast(rhs)}, + container_base{fsm, ::std::move(rhs)} + {} + + state_machine_base_impl(state_machine_base_impl const& rhs) = delete; + state_machine_base_impl(state_machine_base_impl&& rhs) = delete; + + void + swap(state_machine_base_impl& rhs) noexcept + { + using ::std::swap; + static_cast(*this).swap(rhs); + transitions_.swap(rhs.transitions_); + } + + state_machine_base_impl& + operator = (state_machine_base_impl const& rhs) = delete; + state_machine_base_impl& + operator = (state_machine_base_impl&& rhs) = delete; + + template < typename StateDef > + bool + is_in_state() const + { + return is_in_state(state_containment_type{}); + } + + template < ::std::size_t N> + typename ::std::tuple_element< N, inner_states_tuple >::type& + get_state() + { return transitions_.template get_state(); } + template < ::std::size_t N> + typename ::std::tuple_element< N, inner_states_tuple >::type const& + get_state() const + { return transitions_.template get_state(); } + + template < typename StateDef > + typename ::std::enable_if< + !::std::is_same::value && + contains_substate::value, substate_type>::type& + get_state() + { + return get_state(state_containment_type{}); + } + template < typename StateDef > + typename ::std::enable_if< + !::std::is_same::value && + contains_substate::value, substate_type>::type const& + get_state() const + { + return get_state(state_containment_type{}); + } + template < typename StateDef > + typename ::std::enable_if< + ::std::is_same::value, front_machine_type>::type& + get_state() + { + return static_cast(*this); + } + template < typename StateDef > + typename ::std::enable_if< + ::std::is_same::value, front_machine_type>::type const& + get_state() const + { + return static_cast(*this); + } + + ::std::size_t + current_state() const + { return transitions_.current_state(); } + + template < typename Event, typename FSM > + void + state_enter(Event&& event, FSM&) + { + transitions_.enter( ::std::forward(event) ); + } + template < typename Event, typename FSM > + void + state_exit(Event&& event, FSM&) + { + transitions_.exit( ::std::forward(event) ); + } + + event_set + current_handled_events() const + { + auto const& intl = state_type::internal_handled_events(); + auto res = transitions_.current_handled_events(); + res.insert(intl.begin(), intl.end()); + return res; + } + + event_set + current_deferrable_events() const + { + auto const& own = state_type::current_deferrable_events(); + auto res = transitions_.current_deferrable_events(); + res.insert(own.begin(), own.end()); + return res; + } +protected: + template + explicit + state_machine_base_impl(front_machine_type* fsm, Args&& ... args) + : state_type(::std::forward(args)...), + container_base{fsm} + {} + + template < typename FSM, typename Event > + actions::event_process_result + process_event_impl(FSM& enclosing_fsm, Event&& event, + detail::process_type const&) + { + using event_type = typename ::std::decay::type; + using can_handle_in_state = ::std::integral_constant::value>; + // Transitions and internal event dispatch + auto res = transitions_.process_event(::std::forward(event)); + if (res == actions::event_process_result::refuse) { + // Internal transitions + res = process_event_impl(enclosing_fsm, + ::std::forward(event), can_handle_in_state{}); + } + return res; + } + template < typename FSM, typename Event > + constexpr actions::event_process_result + process_event_impl(FSM&, Event&&, + detail::process_type const&) const + { + return actions::event_process_result::defer; + } + template < typename FSM, typename Event > + constexpr actions::event_process_result + process_event_impl(FSM&, Event&&, + detail::process_type const&) const + { + return actions::event_process_result::refuse; + } + + //@{ + /** @name Dispatch of in-state events */ + template < typename FSM, typename Event > + actions::event_process_result + process_event_impl(FSM& enclosing_fsm, Event&& event, + ::std::true_type const& /* Is in-state event */) + { + return actions::handle_in_state_event(::std::forward(event), enclosing_fsm, *this); + } + template < typename FSM, typename Event > + actions::event_process_result + process_event_impl(FSM&, Event&&, + ::std::false_type const& /* Is not an in-state event */ ) + { + return actions::event_process_result::refuse; + } + //@} + //@{ + /** @name Get Substates */ + template < typename StateDef > + substate_type& + get_state(state_containment_immediate const&) + { + using index_of_state = ::psst::meta::index_of; + static_assert(index_of_state::found, + "Type is not a definition of inner state"); + return transitions_.template get_state< index_of_state::value >(); + } + template < typename StateDef > + substate_type const& + get_state(state_containment_immediate const&) const + { + using index_of_state = ::psst::meta::index_of; + static_assert(index_of_state::found, + "Type is not a definition of inner state"); + return transitions_.template get_state< index_of_state::value >(); + } + template < typename StateDef > + substate_type& + get_state(state_containment_substate const&) + { + using search = detail::substate_type; + return get_state< typename search::front >().template get_state(); + } + template < typename StateDef > + substate_type const& + get_state(state_containment_substate const&) const + { + using search = detail::substate_type; + return get_state< typename search::front >().template get_state(); + } + //@} + //@{ + /** @name Is in state check */ + /** + * Constant false for states not contained by this state machine + * @param + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_none const&) const + { return false; } + /** + * Constant true for self + * @param + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_self const&) const + { return true; } + /** + * Is in substate of an inner state + * @param + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_substate const&) const + { + using immediate_states = + typename ::psst::meta::find_if< + def::contains_substate_predicate::template type, + inner_states_def >::type; + return is_in_substate(immediate_states{}); + } + + template < typename StateDef, typename ... ImmediateSubStates > + bool + is_in_substate( ::psst::meta::type_tuple const& ) const + { + return ::psst::meta::any_of({ + (this->template is_in_state() && + this->template get_state< ImmediateSubStates >(). + template is_in_state()) ... + }); + } + /** + * In in an immediately inner state + * @param Template tag switch + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_immediate const&) const + { + using index_of_state = ::psst::meta::index_of; + static_assert(index_of_state::found, + "Type is not a definition of inner state"); + return index_of_state::value == transitions_.current_state(); + } + //@} +protected: + using container_base::transitions_; +}; + +template < typename T, typename Mutex, typename FrontMachine > +constexpr ::std::size_t state_machine_base_impl::initial_state_index; +template < typename T, typename Mutex, typename FrontMachine > +constexpr ::std::size_t state_machine_base_impl::inner_state_count; + +template < typename T, typename Mutex, typename FrontMachine > +struct state_machine_base_with_base : state_machine_base_impl { + using base_type = state_machine_base_impl; + using common_base = typename T::common_base; + + state_machine_base_with_base(FrontMachine* fsm) + : base_type{fsm} + { + } + state_machine_base_with_base(FrontMachine* fsm, state_machine_base_with_base const& rhs) + : base_type{fsm, rhs} + { + } + state_machine_base_with_base(FrontMachine* fsm, state_machine_base_with_base&& rhs) + : base_type{fsm, ::std::move(rhs)} + { + } + + state_machine_base_with_base(state_machine_base_with_base const&) = delete; + state_machine_base_with_base(state_machine_base_with_base&&) = delete; + state_machine_base_with_base& + operator = (state_machine_base_with_base const&) = delete; + state_machine_base_with_base& + operator = (state_machine_base_with_base&&) = delete; + + common_base& + current_state_base() + { + return base_type::transitions_.template cast_current_state(); + } + common_base const& + current_state_base() const + { + return base_type::transitions_.template cast_current_state(); + } +protected: + template + explicit + state_machine_base_with_base(FrontMachine* fsm, Args&& ... args) + : state_machine_base_with_base::machine_type(fsm, ::std::forward(args)...) + {} +}; + +template < typename T, typename Mutex, typename FrontMachine > +class orthogonal_state_machine : public state_base, + public orthogonal::region_container::type> { +public: + using state_machine_definition_type = T; + using state_type = state_base; + using machine_type = orthogonal_state_machine; + using front_machine_type = FrontMachine; + static_assert(def::traits::is_state_machine::value, + "Front state machine can be created only with a descendant of afsm::def::state_machine"); + using orthogonal_regions = typename state_machine_definition_type::orthogonal_regions; + using handled_events = typename def::detail::recursive_handled_events< orthogonal_regions >::type; + using regions_def = typename def::detail::inner_states< orthogonal_regions >::type; + + static constexpr ::std::size_t region_count = regions_def::size; + + using mutex_type = Mutex; + using size_type = typename detail::size_type::type; + + using container_base = + orthogonal::region_container; + using region_tuple = typename container_base::region_tuple; + + template < typename State > + using substate_type = typename detail::substate_type::type; + template < typename State > + using contains_substate = def::contains_substate; +public: + orthogonal_state_machine(front_machine_type* fsm) + : state_type{}, + container_base{fsm} + {} + orthogonal_state_machine(front_machine_type* fsm, orthogonal_state_machine const& rhs) + : state_type{static_cast(rhs)}, + container_base{fsm, rhs} + {} + orthogonal_state_machine(front_machine_type* fsm, orthogonal_state_machine&& rhs) + : state_type{static_cast(rhs)}, + container_base{fsm, ::std::move(rhs)} + {} + + orthogonal_state_machine(orthogonal_state_machine const& rhs) = delete; + orthogonal_state_machine(orthogonal_state_machine&& rhs) = delete; + + void + swap(orthogonal_state_machine& rhs) noexcept + { + using ::std::swap; + static_cast(*this).swap(rhs); + regions_.swap(rhs.regions_); + } + + orthogonal_state_machine& + operator = (orthogonal_state_machine const& rhs) = delete; + orthogonal_state_machine& + operator = (orthogonal_state_machine&& rhs) = delete; + + template < typename StateDef > + bool + is_in_state() const + { + return is_in_state(state_containment_type{}); + } + + template < ::std::size_t N> + typename ::std::tuple_element< N, region_tuple >::type& + get_state() + { return regions_.template get_state(); } + template < ::std::size_t N> + typename ::std::tuple_element< N, region_tuple >::type const& + get_state() const + { return regions_.template get_state(); } + + template < typename StateDef > + typename ::std::enable_if< + !::std::is_same::value && + contains_substate::value, substate_type>::type& + get_state() + { + return get_state(state_containment_type{}); + } + template < typename StateDef > + typename ::std::enable_if< + !::std::is_same::value && + contains_substate::value, substate_type>::type const& + get_state() const + { + return get_state(state_containment_type{}); + } + template < typename StateDef > + typename ::std::enable_if< + ::std::is_same::value, front_machine_type>::type& + get_state() + { + return static_cast(*this); + } + template < typename StateDef > + typename ::std::enable_if< + ::std::is_same::value, front_machine_type>::type const& + get_state() const + { + return static_cast(*this); + } + + template < typename Event, typename FSM > + void + state_enter(Event&& event, FSM&) + { + regions_.enter(::std::forward(event)); + } + template < typename Event, typename FSM > + void + state_exit(Event&& event, FSM&) + { + regions_.exit(::std::forward(event)); + } + event_set + current_handled_events() const + { + auto const& intl = state_type::internal_handled_events(); + event_set res = regions_.current_handled_events(); + res.insert(intl.begin(), intl.end()); + return res; + } + event_set + current_deferrable_events() const + { + auto const& own = state_type::current_deferrable_events(); + auto res = regions_.current_deferrable_events(); + res.insert(own.begin(), own.end()); + return res; + } +protected: + template < typename ... Args > + explicit + orthogonal_state_machine(front_machine_type* fsm, Args&& ... args) + : state_type(::std::forward(args)...), + container_base{fsm} + {} + + template < typename FSM, typename Event > + actions::event_process_result + process_event_impl(FSM& enclosing_fsm, Event&& event, + detail::process_type< actions::event_process_result::process > const&) + { + using event_type = typename ::std::decay::type; + using can_handle_in_state = ::std::integral_constant::value>; + auto res = regions_.process_event(::std::forward(event)); + if (res == actions::event_process_result::refuse) { + // Internal transitions + res = process_event_impl(enclosing_fsm, + ::std::forward(event), can_handle_in_state{}); + } + return res; + } + template < typename FSM, typename Event > + constexpr actions::event_process_result + process_event_impl(FSM&, Event&&, + detail::process_type const&) const + { + return actions::event_process_result::defer; + } + template < typename FSM, typename Event > + constexpr actions::event_process_result + process_event_impl(FSM&, Event&&, + detail::process_type const&) const + { + return actions::event_process_result::refuse; + } + //@{ + /** @name Dispatch of in-state events */ + template < typename FSM, typename Event > + actions::event_process_result + process_event_impl(FSM& enclosing_fsm, Event&& event, + ::std::true_type const& /* Is in-state event */) + { + return actions::handle_in_state_event(::std::forward(event), enclosing_fsm, *this); + } + template < typename FSM, typename Event > + actions::event_process_result + process_event_impl(FSM&, Event&&, + ::std::false_type const& /* Is not an in-state event */ ) + { + return actions::event_process_result::refuse; + } + //@} + //@{ + /** @name Get Substates */ + template < typename StateDef > + substate_type& + get_state(state_containment_immediate const&) + { + using index_of_state = ::psst::meta::index_of; + static_assert(index_of_state::found, + "Type is not a definition of inner state"); + return regions_.template get_state< index_of_state::value >(); + } + template < typename StateDef > + substate_type const& + get_state(state_containment_immediate const&) const + { + using index_of_state = ::psst::meta::index_of; + static_assert(index_of_state::found, + "Type is not a definition of inner state"); + return regions_.template get_state< index_of_state::value >(); + } + template < typename StateDef > + substate_type& + get_state(state_containment_substate const&) + { + using search = detail::substate_type; + return get_state< typename search::front >().template get_state(); + } + template < typename StateDef > + substate_type const& + get_state(state_containment_substate const&) const + { + using search = detail::substate_type; + return get_state< typename search::front >().template get_state(); + } + //@} + //@{ + /** @name Is in state checks */ + /** + * Constant false for states not contained by this state machine + * @param + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_none const&) const + { return false; } + /** + * Constant true for self + * @param + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_self const&) const + { return true; } + /** + * Is in substate of an inner state + * @param + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_substate const&) const + { + using immediate_states = + typename ::psst::meta::find_if< + def::contains_substate_predicate::template type, + regions_def >::type; + return is_in_substate(immediate_states{}); + } + + template < typename StateDef, typename ... ImmediateSubStates > + bool + is_in_substate( ::psst::meta::type_tuple const& ) const + { + return ::psst::meta::any_of({ + (this->template is_in_state() && + this->template get_state< ImmediateSubStates >(). + template is_in_state()) ... + }); + } + /** + * In in an immediately inner state. All immediate states are regions + * and are always active. + * @param Template tag switch + * @return + */ + template < typename StateDef > + bool + is_in_state(state_containment_immediate const&) const + { + return true; + } + //@} +protected: + using container_base::regions_; +}; + +template < typename T, typename Mutex, typename FrontMachine > +constexpr ::std::size_t orthogonal_state_machine::region_count; + +template < typename T, typename Mutex, typename FrontMachine > +struct state_machine_base : ::std::conditional< + def::traits::has_orthogonal_regions::value, + orthogonal_state_machine, // TODO Check common base + typename ::std::conditional< + def::traits::has_common_base::value, + state_machine_base_with_base< T, Mutex, FrontMachine >, + state_machine_base_impl< T, Mutex, FrontMachine > + >::type + >::type { +public: + using state_machine_impl_type = typename ::std::conditional< + def::traits::has_orthogonal_regions::value, + orthogonal_state_machine, + typename ::std::conditional< + def::traits::has_common_base::value, + state_machine_base_with_base< T, Mutex, FrontMachine >, + state_machine_base_impl< T, Mutex, FrontMachine > + >::type + >::type; + state_machine_base(FrontMachine* fsm) + : state_machine_impl_type{fsm} {} + state_machine_base(FrontMachine* fsm, state_machine_base const& rhs) + : state_machine_impl_type{fsm, rhs} {} + state_machine_base(FrontMachine* fsm, state_machine_base&& rhs) + : state_machine_impl_type{fsm, ::std::move(rhs)} {} + + state_machine_base(state_machine_base const&) = delete; + state_machine_base(state_machine_base&&) = delete; + state_machine_base& + operator = (state_machine_base const&) = delete; + state_machine_base& + operator = (state_machine_base&&) = delete; +protected: + template + explicit + state_machine_base(FrontMachine* fsm, Args&& ... args) + : state_machine_impl_type(fsm, ::std::forward(args)...) {} +}; + +} /* namespace detail */ +} /* namespace afsm */ + + + +#endif /* AFSM_DETAIL_BASE_STATES_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/debug_io.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/debug_io.hpp new file mode 100644 index 0000000000..27fb6fb539 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/debug_io.hpp @@ -0,0 +1,46 @@ +/* + * debug_io.hpp + * + * Created on: 2 июня 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_DEBUG_IO_HPP_ +#define AFSM_DETAIL_DEBUG_IO_HPP_ + +#include +#include + +namespace afsm { +namespace actions { + +inline ::std::ostream& +operator << (::std::ostream& os, event_process_result const& val) +{ + ::std::ostream::sentry s (os); + if (s) { + switch (val) { + case event_process_result::refuse: + os << "refuse"; + break; + case event_process_result::process: + os << "transit"; + break; + case event_process_result::process_in_state: + os << "in-state"; + break; + case event_process_result::defer: + os << "defer"; + break; + default: + break; + } + } + return os; +} + + +} /* namespace actions */ +} /* namespace afsm */ + +#endif /* AFSM_DETAIL_DEBUG_IO_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/def_traits.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/def_traits.hpp new file mode 100644 index 0000000000..2d9668bd36 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/def_traits.hpp @@ -0,0 +1,157 @@ +/* + * def_traits.hpp + * + * Created on: 1 июня 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_DEF_TRAITS_HPP_ +#define AFSM_DETAIL_DEF_TRAITS_HPP_ + +#include +#include +#include + +namespace afsm { +namespace def { +namespace traits { + +template < typename T > +struct is_transition : ::std::false_type {}; + +template < typename SourceState, typename Event, typename TargetState, + typename Action, typename Guard> +struct is_transition< transition > + : ::std::true_type{}; +template < typename Event, typename Action, typename Guard > +struct is_transition< internal_transition< Event, Action, Guard > > + : ::std::true_type {}; + +template < typename T > +struct is_internal_transition : ::std::false_type {}; + +template < typename Event, typename Action, typename Guard > +struct is_internal_transition< internal_transition< Event, Action, Guard > > + : ::std::true_type {}; + +template < typename T > +struct is_state + : ::std::is_base_of< tags::state, T > {}; + +template < typename T > +struct is_terminal_state + : ::std::is_base_of< terminal_state, T > {}; + +template < typename T > +struct is_state_machine + : ::std::is_base_of< tags::state_machine, T > {}; + +template < typename T > +struct is_pushdown + : ::std::is_base_of< tags::pushdown_state, T > {}; +template < typename T > +struct is_popup + : ::std::is_base_of< tags::popup_state, T > {}; + +namespace detail { + +template < typename T, typename Machine, bool IsPush > +struct pushes : ::std::false_type {}; + +template < typename T, typename Machine > +struct pushes + : ::std::integral_constant::value> {}; + +template < typename T, typename Machine, bool IsPop > +struct pops : ::std::false_type {}; + +template < typename T, typename Machine > +struct pops + : ::std::integral_constant::value> {}; + +} /* namespace detail */ + +template < typename T, typename Machine > +struct pushes : detail::pushes::value> {}; +template < typename T, typename Machine > +struct pops : detail::pops::value> {}; + +template < typename T > +struct has_common_base + : ::std::is_base_of< tags::has_common_base, T > {}; + +template < typename T > +struct has_history + : ::std::is_base_of< tags::has_history, T > {}; + +template < typename T > +struct allow_empty_transition_functions + : ::std::is_base_of< tags::allow_empty_enter_exit, T > {}; + +template < typename T > +struct has_orthogonal_regions + : ::std::integral_constant::value> {}; + +template < typename T > +struct exception_safety { + using type = typename ::std::conditional< + ::std::is_base_of::value, + tags::nothrow_guarantee, + typename ::std::conditional< + ::std::is_base_of::value, + tags::strong_exception_safety, + tags::basic_exception_safety + >::type + >::type; +}; + +namespace detail { +template < typename T, bool HasCommonBase > +struct inner_states_def { + using definition_type = T; + using common_base_tag = typename definition_type::common_base_tag_type; + using exception_guarantee = typename exception_safety::type; + + template < typename U, typename ... Tags > + using state = def::state_def; + template < typename U, typename ... Tags > + using terminal_state = def::terminal_state; + template < typename U, typename ... Tags > + using state_machine = def::state_machine; + template < typename U, typename M, typename ... Tags > + using push = def::pushdown; + template < typename U, typename M, typename ... Tags > + using pop = def::popup; +}; + +template < typename T > +struct inner_states_def { + using definition_type = T; + using common_base_type = void; + using exception_guarantee = typename exception_safety::type; + + template < typename U, typename ... Tags > + using state = def::state_def; + template < typename U, typename ... Tags > + using terminal_state = def::terminal_state; + template < typename U, typename ... Tags > + using state_machine = def::state_machine; + template < typename U, typename M, typename ... Tags > + using push = def::pushdown; + template < typename U, typename M, typename ... Tags > + using pop = def::popup; +}; + +} /* namespace detail */ + +template < typename T > +struct inner_states_definitions + : detail::inner_states_def::value> {}; + +} /* namespace traits */ +} /* namespace def */ +} /* namespace afsm */ + +#endif /* AFSM_DETAIL_DEF_TRAITS_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/event_identity.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/event_identity.hpp new file mode 100644 index 0000000000..3ce7ed4adf --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/event_identity.hpp @@ -0,0 +1,52 @@ +/* + * event_identity.hpp + * + * Created on: Dec 20, 2016 + * Author: zmij + */ + +#ifndef AFSM_DETAIL_EVENT_IDENTITY_HPP_ +#define AFSM_DETAIL_EVENT_IDENTITY_HPP_ + +#include +#include +#include + +namespace afsm { +namespace detail { + +struct event_base { + struct id_type {}; +}; + +template < typename T > +struct event : event_base { + static constexpr id_type id{}; +}; + +template < typename T > +constexpr event_base::id_type event::id; + +template < typename T > +struct event_identity { + using event_type = typename ::std::decay::type; + using type = event; +}; + +using event_set = ::std::set< event_base::id_type const* >; + +template < typename ... T > +event_set +make_event_set( ::psst::meta::type_tuple const& ) +{ + return event_set{ + &event_identity::type::id... + }; +} + +} /* namespace detail */ +} /* namespace afsm */ + + + +#endif /* AFSM_DETAIL_EVENT_IDENTITY_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/exception_safety_guarantees.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/exception_safety_guarantees.hpp new file mode 100644 index 0000000000..bfed1b5595 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/exception_safety_guarantees.hpp @@ -0,0 +1,46 @@ +/* + * exception_safety_guarantees.hpp + * + * Created on: Nov 30, 2016 + * Author: zmij + */ + +#ifndef AFSM_DETAIL_EXCEPTION_SAFETY_GUARANTEES_HPP_ +#define AFSM_DETAIL_EXCEPTION_SAFETY_GUARANTEES_HPP_ + + +namespace afsm { +namespace def { +namespace tags { + +/** + * Default exception safety guarantee + * On an exception caused by + * * a guard check nothing happens + * * state exit action - only data modified by exit action before exception + * * transition action - previous and data modified by the transition action + * * state enter action - previous and data modified by the enter action + * * source state constructor - all of the above + */ +struct basic_exception_safety {}; +/** + * Strong exception safety guarantee + * If an exception is thrown in the course of transition, nothing will be modified. + * + * Requires non-throwing swap operations + * The exception will be thrown. + */ +struct strong_exception_safety {}; +/** + * Same as the strong exception safety, but the exception will be consumed + * The event will be rejected and further behavior is ruled by event rejection + * policies. + */ +struct nothrow_guarantee {}; + +} /* namespace tags */ +} /* namespace def */ +} /* namespace afsm */ + + +#endif /* AFSM_DETAIL_EXCEPTION_SAFETY_GUARANTEES_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/helpers.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/helpers.hpp new file mode 100644 index 0000000000..0cc0302fb0 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/helpers.hpp @@ -0,0 +1,195 @@ +/* + * helpers.hpp + * + * Created on: 29 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_HELPERS_HPP_ +#define AFSM_DETAIL_HELPERS_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace afsm { +namespace detail { + +template +struct not_a_state; + +template < typename T > +struct not_a_state { + static_assert( def::traits::is_state::value, "Type is not a state" ); +}; + +template < typename T, typename FSM > +struct front_state_type + : ::std::conditional< + def::traits::is_state_machine< T >::value, + inner_state_machine< T, FSM >, + typename ::std::conditional< + def::traits::is_state< T >::value, + state< T, FSM >, + not_a_state< T > + >::type + > {}; + +template < typename T, typename ... Y, typename FSM > +struct front_state_type<::psst::meta::type_tuple, FSM> + : front_state_type< + ::psst::meta::type_tuple, + typename front_state_type< T, FSM >::type > {}; + +template < typename T, typename FSM > +struct front_state_type<::psst::meta::type_tuple, FSM> + : front_state_type {}; + +template < typename FSM, typename T > +struct front_state_tuple; + +template < typename FSM > +struct front_state_tuple< FSM, void> { + using type = ::std::tuple<>; + + static type + construct(FSM&) + { return type{}; } +}; + +template < typename FSM, typename ... T> +struct front_state_tuple< FSM, ::psst::meta::type_tuple > { + using type = ::std::tuple< typename front_state_type::type ... >; + using index_tuple = typename ::psst::meta::index_builder< sizeof ... (T) >::type; + + static type + construct(FSM& fsm) + { return type( typename front_state_type::type{fsm}... ); } + static type + copy_construct(FSM& fsm, type const& rhs) + { + return copy_construct(fsm, rhs, index_tuple{}); + } + static type + move_construct(FSM& fsm, type&& rhs) + { + return move_construct(fsm, ::std::forward(rhs), index_tuple{}); + } +private: + template < ::std::size_t ... Indexes > + static type + copy_construct(FSM& fsm, type const& rhs, ::psst::meta::indexes_tuple const&) + { + return type( typename front_state_type::type{ + fsm, ::std::get< Indexes >(rhs)}...); + } + template < ::std::size_t ... Indexes > + static type + move_construct(FSM& fsm, type&& rhs, ::psst::meta::indexes_tuple const&) + { + return type( typename front_state_type::type{ + fsm, ::std::move(::std::get< Indexes >(rhs))}...); + } +}; + +template < typename FSM, typename State, bool Contains > +struct substate_type_impl { + using full_path = typename def::state_path::type; + using path = typename ::psst::meta::pop_front::type; + using type = typename front_state_type::type; + using front = typename ::psst::meta::front::type; +}; + +template < typename FSM, typename State, bool IsSelf > +struct substate_type_self { + using type = FSM; +}; + +template < typename FSM, typename State > +struct substate_type_self {}; + +template < typename FSM, typename State > +struct substate_type_impl + : substate_type_self ::value> {}; + +template < typename FSM, typename State > +struct substate_type + : substate_type_impl::value>{}; + +template < typename FSM, typename StateTable > +struct stack_constructor { + using state_table_type = StateTable; + using stack_item = state_table_type; + using type = ::std::deque; + static constexpr ::std::size_t size = state_table_type::size; + + static type + construct(FSM& fsm) + { + type res; + res.emplace_back(state_table_type{fsm}); + return res; + } + static type + copy_construct(FSM& fsm, type const& rhs) + { + type res; + ::std::transform(rhs.begin(), rhs.end(), ::std::back_inserter(res), + [fsm](stack_item const& item) + { + return stack_item{ fsm, item }; + }); + return res; + } + static type + move_construct(FSM& fsm, type&& rhs) + { + type res; + ::std::transform(rhs.begin(), rhs.end(), ::std::back_inserter(res), + [fsm](stack_item&& item) + { + return stack_item{fsm, ::std::move(item) }; + }); + return res; + } +}; + +struct no_lock { + no_lock(none&) {} +}; + +template < typename Mutex > +struct lock_guard_type { + using type = ::std::lock_guard; +}; + +template <> +struct lock_guard_type { + using type = no_lock; +}; +template <> +struct lock_guard_type { + using type = no_lock; +}; + +template +struct size_type { + using type = ::std::atomic<::std::size_t>; +}; + +template <> +struct size_type { + using type = ::std::size_t; +}; +template <> +struct size_type { + using type = ::std::size_t; +}; + +} /* namespace detail */ +} /* namespace afsm */ + +#endif /* AFSM_DETAIL_HELPERS_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/observer.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/observer.hpp new file mode 100644 index 0000000000..4c7aa52ecc --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/observer.hpp @@ -0,0 +1,238 @@ +/* + * observer.hpp + * + * Created on: Jun 3, 2016 + * Author: zmij + */ + +#ifndef AFSM_DETAIL_OBSERVER_HPP_ +#define AFSM_DETAIL_OBSERVER_HPP_ + +#include +#include +#include +#include + +namespace afsm { +namespace detail { + +struct null_observer { + template < typename FSM, typename Event > + void + start_process_event(FSM const&, Event const&) const noexcept {} + + template < typename FSM, typename State, typename Event > + void + state_entered(FSM const&, State const&, Event const&) const noexcept{} + template < typename FSM, typename State, typename Event > + void + state_exited(FSM const&, State const&, Event const&) const noexcept{} + template < typename FSM, typename State > + void + state_cleared(FSM const&, State const&) const noexcept{} + + template < typename FSM, typename SourceState, typename TargetState, typename Event> + void + state_changed(FSM const&, SourceState const&, TargetState const&, Event const&) const noexcept {} + + template < typename FSM, typename Event > + void + processed_in_state(FSM const&, Event const&) const noexcept {} + + template < typename FSM, typename Event > + void + enqueue_event(FSM const&, Event const&) const noexcept {} + + template < typename FSM > + void + start_process_events_queue(FSM const&) const noexcept {} + template < typename FSM > + void + end_process_events_queue(FSM const&) const noexcept {} + + template < typename FSM, typename Event > + void + defer_event(FSM const&, Event const&) const noexcept {} + + template < typename FSM > + void + start_process_deferred_queue(FSM const&, ::std::size_t /*size*/) const noexcept {} + template < typename FSM > + void + end_process_deferred_queue(FSM const&, ::std::size_t /*remain*/) const noexcept {} + + template < typename FSM > + void + skip_processing_deferred_queue(FSM const&) const noexcept {} + template < typename FSM > + void + postpone_deferred_events(FSM const&, ::std::size_t /*count*/) const noexcept {} + template < typename FSM > + void + drop_deferred_event(FSM const&) const noexcept{} + + template < typename FSM, typename Event > + void + reject_event(FSM const&, Event const&) const noexcept {} +}; + +template < typename T > +class observer_wrapper { +public: + using observer_ptr = ::std::shared_ptr; +public: + observer_wrapper() + : observer_{} {} + void + set_observer(observer_ptr observer) + { + observer_ = observer; + } + + template < typename ... Args > + void + make_observer(Args&& ... args) + { + observer_ = ::std::make_shared(::std::forward(args)...); + } +protected: + template < typename FSM, typename FSM_DEF, typename Size > + friend class transitions::state_transition_table; + + template < typename FSM, typename Event > + void + start_process_event(FSM const& fsm, Event const& event) const noexcept + { + if (observer_) + observer_->start_process_event(fsm, event); + } + + template < typename FSM, typename State, typename Event > + void + state_entered(FSM const& fsm, State const& state, Event const& event) const noexcept + { + if (observer_) + observer_->state_entered(fsm, state, event); + } + template < typename FSM, typename State, typename Event > + void + state_exited(FSM const& fsm, State const& state, Event const& event) const noexcept + { + if (observer_) + observer_->state_exited(fsm, state, event); + } + template < typename FSM, typename State > + void + state_cleared(FSM const& fsm, State const& state) const noexcept + { + if (observer_) + observer_->state_cleared(fsm, state); + } + template < typename FSM, typename SourceState, typename TargetState, typename Event> + void + state_changed(FSM const& fsm, SourceState const& source, + TargetState const& target, Event const& event) const noexcept + { + if (observer_) + observer_->state_changed(fsm, source, target, event); + } + + template < typename FSM, typename Event > + void + processed_in_state(FSM const& fsm, Event const& event) const noexcept + { + if (observer_) + observer_->processed_in_state(fsm, event); + } + + template < typename FSM, typename Event > + void + enqueue_event(FSM const& fsm, Event const& event) const noexcept + { + if (observer_) + observer_->enqueue_event(fsm, event); + } + + template < typename FSM > + void + start_process_events_queue(FSM const& fsm) const noexcept + { + if (observer_) + observer_->start_process_events_queue(fsm); + } + + template < typename FSM > + void + end_process_events_queue(FSM const& fsm) const noexcept + { + if (observer_) + observer_->end_process_events_queue(fsm); + } + + template < typename FSM, typename Event > + void + defer_event(FSM const& fsm, Event const& event) const noexcept + { + if (observer_) + observer_->defer_event(fsm, event); + } + + template < typename FSM > + void + start_process_deferred_queue(FSM const& fsm, ::std::size_t size) const noexcept + { + if (observer_) + observer_->start_process_deferred_queue(fsm, size); + } + + template < typename FSM > + void + end_process_deferred_queue(FSM const& fsm, ::std::size_t remain) const noexcept + { + if (observer_) + observer_->end_process_deferred_queue(fsm, remain); + } + + template < typename FSM > + void + skip_processing_deferred_queue(FSM const& fsm) const noexcept + { + if (observer_) + observer_->skip_processing_deferred_queue(fsm); + } + template < typename FSM > + void + postpone_deferred_events(FSM const& fsm, ::std::size_t count) const noexcept + { + if (observer_) + observer_->postpone_deferred_events(fsm, count); + } + template < typename FSM > + void + drop_deferred_event(FSM const& fsm) const noexcept + { + if (observer_) + observer_->drop_deferred_event(fsm); + } + + template < typename FSM, typename Event > + void + reject_event(FSM const& fsm, Event const& event) const noexcept + { + if (observer_) + observer_->reject_event(fsm, event); + } +private: + observer_ptr observer_; +}; + +template <> +struct observer_wrapper : null_observer { +}; + +} /* namespace detail */ +} /* namespace afsm */ + + + +#endif /* AFSM_DETAIL_OBSERVER_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/orthogonal_regions.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/orthogonal_regions.hpp new file mode 100644 index 0000000000..3c49e8a38a --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/orthogonal_regions.hpp @@ -0,0 +1,474 @@ +/* + * orthogonal_regions.hpp + * + * Created on: 27 нояб. 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_ORTHOGONAL_REGIONS_HPP_ +#define AFSM_DETAIL_ORTHOGONAL_REGIONS_HPP_ + +#include +#include + +namespace afsm { +namespace orthogonal { + +namespace detail { + +template < ::std::size_t N > +struct invoke_nth { + using previous = invoke_nth; + static constexpr ::std::size_t index = N; + using event_handler_type = actions::detail::process_event_handler; + using event_set = ::afsm::detail::event_set; + + template < typename Regions, typename Event, typename FSM > + static void + enter(Regions& regions, Event&& event, FSM& fsm) + { + using state_type = typename ::std::tuple_element::type; + using state_enter = transitions::detail::state_enter; + + previous::enter(regions, ::std::forward(event), fsm); + state_enter{}(::std::get(regions), ::std::forward(event), fsm); + } + + template < typename Regions, typename Event, typename FSM > + static void + exit(Regions& regions, Event&& event, FSM& fsm) + { + using state_type = typename ::std::tuple_element::type; + using state_exit = transitions::detail::state_exit; + + // Reverse order of exit + state_exit{}(::std::get(regions), ::std::forward(event), fsm); + previous::exit(regions, ::std::forward(event), fsm); + } + + template < typename Regions, typename Event > + static actions::event_process_result + process_event(Regions& regions, Event&& event) + { + auto res = previous::process_event(regions, ::std::forward(event)); + return ::std::max(res, event_handler_type{}(regions, ::std::forward(event))); + } + + template < typename Regions > + static void + collect_events( Regions const& regions, event_set& events ) + { + previous::collect_events(regions, events); + auto const& region = ::std::get(regions); + event_set evts = region.current_handled_events(); + events.insert(evts.begin(), evts.end()); + } + template < typename Regions > + static void + collect_deferred_events( Regions const& regions, event_set& events ) + { + previous::collect_deferred_events(regions, events); + auto const& region = ::std::get(regions); + event_set evts = region.current_deferrable_events(); + events.insert(evts.begin(), evts.end()); + } +}; + +template <> +struct invoke_nth< 0 > { + static constexpr ::std::size_t index = 0; + using event_handler_type = actions::detail::process_event_handler; + using event_set = ::afsm::detail::event_set; + + template < typename Regions, typename Event, typename FSM > + static void + enter(Regions& regions, Event&& event, FSM& fsm) + { + using state_type = typename ::std::tuple_element::type; + using state_enter = transitions::detail::state_enter; + state_enter{}(::std::get(regions), ::std::forward(event), fsm); + } + + template < typename Regions, typename Event, typename FSM > + static void + exit(Regions& regions, Event&& event, FSM& fsm) + { + using state_type = typename ::std::tuple_element::type; + using state_exit = transitions::detail::state_exit; + state_exit{}(::std::get(regions), ::std::forward(event), fsm); + } + + template < typename Regions, typename Event > + static actions::event_process_result + process_event(Regions& regions, Event&& event) + { + return event_handler_type{}(regions, ::std::forward(event)); + } + + template < typename Regions > + static void + collect_events( Regions const& regions, event_set& events ) + { + auto const& region = ::std::get(regions); + region.current_handled_events().swap(events); + } + template < typename Regions > + static void + collect_deferred_events( Regions const& regions, event_set& events ) + { + auto const& region = ::std::get(regions); + region.current_deferrable_events().swap(events); + } +}; + +} /* namespace detail */ + +template < typename FSM, typename FSM_DEF, typename Size > +class regions_table { +public: + using fsm_type = FSM; + using state_machine_definition_type = FSM_DEF; + using size_type = Size; + + using this_type = regions_table; + + using orthogonal_regions = typename state_machine_definition_type::orthogonal_regions; + static_assert(!::std::is_same::value, + "Orthogonal regions table is not defined for orthogonal state machine"); + + using regions_def = typename def::detail::inner_states< orthogonal_regions >::type; + using regions_constructor = ::afsm::detail::front_state_tuple; + using regions_tuple = typename regions_constructor::type; + + static constexpr ::std::size_t size = regions_def::size; + + using region_indexes = typename ::psst::meta::index_builder::type; + using all_regions = detail::invoke_nth; + using event_set = ::afsm::detail::event_set; +public: + regions_table(fsm_type& fsm) + : fsm_{&fsm}, + regions_{ regions_constructor::construct(fsm) } + {} + regions_table(fsm_type& fsm, regions_table const& rhs) + : fsm_{&fsm}, + regions_{ regions_constructor::copy_construct(fsm, rhs.regions_) } + {} + regions_table(fsm_type& fsm, regions_table&& rhs) + : fsm_{&fsm}, + regions_{ regions_constructor::move_construct(fsm, ::std::move(rhs.regions_)) } + {} + + regions_table(regions_table const&) = delete; + regions_table(regions_table&& rhs) + : fsm_{rhs.fsm_}, + regions_{::std::move(rhs.regions_)} + { + } + regions_table& + operator = (regions_table const&) = delete; + regions_table& + operator = (regions_table&&) = delete; + + void + swap(regions_table& rhs) + { + using ::std::swap; + swap(regions_, rhs.regions_); + set_fsm(*fsm_); + rhs.set_fsm(*rhs.fsm_); + } + + void + set_fsm(fsm_type& fsm) + { + fsm_ = &fsm; + ::afsm::detail::set_enclosing_fsm::set(fsm, regions_); + } + + regions_tuple& + regions() + { return regions_; } + regions_tuple const& + regions() const + { return regions_; } + + template < ::std::size_t N> + typename ::std::tuple_element< N, regions_tuple >::type& + get_state() + { return ::std::get(regions_); } + template < ::std::size_t N> + typename ::std::tuple_element< N, regions_tuple >::type const& + get_state() const + { return ::std::get(regions_); } + + template < typename Event > + actions::event_process_result + process_event(Event&& event) + { + // Pass event to all regions + return all_regions::process_event(regions_, ::std::forward(event)); + } + event_set + current_handled_events() const + { + event_set evts; + all_regions::collect_events(regions_, evts); + return evts; + } + event_set + current_deferrable_events() const + { + event_set evts; + all_regions::collect_deferred_events(regions_, evts); + return evts; + } + + template < typename Event > + void + enter(Event&& event) + { + // Call enter for all regions + all_regions::enter(regions_, ::std::forward(event), *fsm_); + } + template < typename Event > + void + exit(Event&& event) + { + // Call exit for all regions + all_regions::exit(regions_, ::std::forward(event), *fsm_); + } +private: + fsm_type* fsm_; + regions_tuple regions_; +}; + +template < typename FSM, typename FSM_DEF, typename Size > +class regions_stack { +public: + using region_table_type = regions_table; + using this_type = regions_stack; + + using fsm_type = typename region_table_type::fsm_type; + using state_machine_definition_type = typename region_table_type::state_machine_definition_type; + using size_type = typename region_table_type::size_type; + using regions_tuple = typename region_table_type::regions_tuple; + + using stack_constructor_type = afsm::detail::stack_constructor; + using event_set = ::afsm::detail::event_set; +public: + regions_stack(fsm_type& fsm) + : fsm_{&fsm}, + state_stack_{ stack_constructor_type::construct(fsm) } + {} + regions_stack(fsm_type& fsm, regions_stack const& rhs) + : fsm_{&fsm}, + state_stack_{ stack_constructor_type::copy_construct(fsm, rhs.state_stack_) } + {} + regions_stack(fsm_type& fsm, regions_stack&& rhs) + : fsm_{&fsm}, + state_stack_{ stack_constructor_type::move_construct(fsm, ::std::move(rhs.state_stack_)) } + {} + + regions_stack(regions_stack const&) = delete; + regions_stack(regions_stack&&) = delete; + regions_stack& + operator = (regions_stack const&) = delete; + regions_stack& + operator = (regions_stack&&) = delete; + + void + swap(regions_stack& rhs) + { + using ::std::swap; + swap(state_stack_, rhs.state_stack_); + set_fsm(*fsm_); + rhs.set_fsm(*rhs.fsm_); + } + + void + set_fsm(fsm_type& fsm) + { + fsm_ = &fsm; + for (auto& item : state_stack_) { + item.set_fsm(fsm); + } + } + + regions_tuple& + states() + { return top().states(); } + regions_tuple const& + states() const + { return top().states(); } + + template < ::std::size_t N > + typename ::std::tuple_element< N, regions_tuple >::type& + get_state() + { return top().template get_state(); } + template < ::std::size_t N > + typename ::std::tuple_element< N, regions_tuple >::type const& + get_state() const + { return top().template get_state(); } + + template < typename Event > + actions::event_process_result + process_event(Event&& event) + { + return top().process_event(::std::forward(event)); + } + event_set + current_handled_events() const + { + return top().current_handled_events(); + } + event_set + current_deferrable_events() const + { + return top().current_deferrable_events(); + } + + template < typename Event > + void + enter(Event&& event) + { + top().enter(::std::forward(event)); + } + template < typename Event > + void + exit(Event&& event) + { + top().exit(::std::forward(event)); + } + + template < typename Event > + void + push(Event&& event) + { + state_stack_.emplace_back( *fsm_ ); + enter(::std::forward(event)); + } + + template < typename Event > + void + pop(Event&& event) + { + if (state_stack_.size() > 1) { + exit(::std::forward(event)); + state_stack_.pop_back(); + } + } + + ::std::size_t + stack_size() const + { + return state_stack_.size(); + } +private: + using stack_type = typename stack_constructor_type::type; + + region_table_type& + top() + { return state_stack_.back(); } + region_table_type const& + top() const + { return state_stack_.back(); } +private: + fsm_type *fsm_; + stack_type state_stack_; +}; + +template < typename FSM, typename FSM_DEF, typename Size, bool HasPushdowns > +struct region_container_selector { + using type = regions_table; +}; + +template < typename FSM, typename FSM_DEF, typename Size > +struct region_container_selector { + using type = regions_stack; +}; + +namespace detail { + +template < typename FSM, typename FSM_DEF, typename Size, bool HasPushdowns > +struct region_container { + using region_tuple = typename region_container_selector::type; + + region_container(FSM* fsm) + : regions_{*fsm} + {} + region_container(FSM* fsm, region_container const& rhs) + : regions_{*fsm, rhs.regions_} + {} + region_container(FSM* fsm, region_container&& rhs) + : regions_{*fsm, ::std::move(rhs.regions_)} + {} +protected: + region_tuple regions_; +}; + +template < typename FSM, typename FSM_DEF, typename Size > +struct region_container { + using region_tuple = typename region_container_selector::type; + + region_container(FSM* fsm) + : regions_{*fsm} + {} + region_container(FSM* fsm, region_container const& rhs) + : regions_{*fsm, rhs.regions_} + {} + region_container(FSM* fsm, region_container&& rhs) + : regions_{*fsm, ::std::move(rhs.regions_)} + {} + + ::std::size_t + stack_size() const + { return regions_.stack_size(); } + +protected: + template < typename T > + friend struct ::afsm::detail::pushdown_state; + template < typename T > + friend struct ::afsm::detail::popup_state; + // Push/pop ops + template < typename Event > + void + pushdown(Event&& event) + { + regions_.push(::std::forward(event)); + } + template < typename Event > + void + popup(Event&& event) + { + regions_.pop(::std::forward(event)); + } +protected: + region_tuple regions_; +}; + +} /* namespace detail */ + +template < typename FSM, typename FSM_DEF, typename Size > +struct region_container + : detail::region_container::value> { + using base_type = detail::region_container::value>; + using region_tuple = typename base_type::region_tuple; + + region_container(FSM* fsm) : base_type{fsm} {} + region_container(FSM* fsm, region_container const& rhs) + : base_type{fsm, rhs} {} + region_container(FSM* fsm, region_container&& rhs) + : base_type{fsm, ::std::move(rhs)} {} +protected: + using base_type::regions_; +}; + +} /* namespace orthogonal */ +} /* namespace afsm */ + + + +#endif /* AFSM_DETAIL_ORTHOGONAL_REGIONS_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/reject_policies.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/reject_policies.hpp new file mode 100644 index 0000000000..55f21af713 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/reject_policies.hpp @@ -0,0 +1,45 @@ +/* + * reject_policies.hpp + * + * Created on: Nov 30, 2016 + * Author: zmij + */ + +#ifndef AFSM_DETAIL_REJECT_POLICIES_HPP_ +#define AFSM_DETAIL_REJECT_POLICIES_HPP_ + +#include +#include +#include + +namespace afsm { +namespace def { +namespace tags { + +struct reject_throw_event { + template < typename Event, typename FSM > + actions::event_process_result + reject_event(Event&& event, FSM&) + { + throw event; + } +}; + +struct reject_throw { + template < typename Event, typename FSM > + actions::event_process_result + reject_event(Event&&, FSM&) + { + using ::psst::util::demangle; + throw ::std::runtime_error{ + "An instance of " + demangle() + " event was rejected" + }; + } +}; + +} /* namespace tags */ +} /* namespace def */ +} /* namespace afsm */ + + +#endif /* AFSM_DETAIL_REJECT_POLICIES_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/tags.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/tags.hpp new file mode 100644 index 0000000000..2071872089 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/tags.hpp @@ -0,0 +1,89 @@ +/* + * tags.hpp + * + * Created on: 1 июня 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_TAGS_HPP_ +#define AFSM_DETAIL_TAGS_HPP_ + +namespace afsm { +namespace def { +namespace tags { + +/** + * Tag for state class. + * For internal use only. + */ +struct state {}; +/** + * Tag for state machine class. + * For internal use only. + */ +struct state_machine{}; + +/** + * Tag for marking states with history. + */ +struct has_history {}; +/** + * Tag for marking states having common base class. + * For internal use. + */ +struct has_common_base {}; + +/** + * Tag for marking state having common base. + */ +template < typename T > +struct common_base : T, has_common_base { + using common_base_type = T; + using common_base_tag_type = common_base; +}; + +template <> +struct common_base { + using common_base_type = void; + using common_base_tag_type = common_base; +}; + +/** + * Tag for marking state having common virtual base. + */ +template < typename T > +struct virtual_common_base : virtual T, has_common_base { + using common_base_type = T; + using common_base_tag_type = virtual_common_base; +}; + +template <> +struct virtual_common_base { + using common_base_type = void; + using common_base_tag_type = virtual_common_base; +}; + +//@{ +/** @name Push/pop tags */ +struct pushdown_state {}; +struct popup_state {}; + +template < typename T > +struct pushdown : pushdown_state { + using pushdown_machine_type = T; +}; + +template < typename T > +struct popup : popup_state { + using pushdown_machine_type = T; +}; +//@} + +struct allow_empty_enter_exit {}; +struct mandatory_empty_enter_exit {}; + +} /* namespace tags */ +} /* namespace def */ +} /* namespace afsm */ + +#endif /* AFSM_DETAIL_TAGS_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp new file mode 100644 index 0000000000..e4fe1fa821 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp @@ -0,0 +1,1018 @@ +/* + * transitions.hpp + * + * Created on: 29 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_DETAIL_TRANSITIONS_HPP_ +#define AFSM_DETAIL_TRANSITIONS_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace afsm { +namespace transitions { + +namespace detail { + +enum class event_handle_type { + none, + transition, + internal_transition, + inner_state, +}; + +template +using handle_event = ::std::integral_constant; + +template < typename FSM, typename Event > +struct event_handle_selector + : ::std::conditional< + (::psst::meta::find_if< + def::handles_event::template type, + typename FSM::transitions >::type::size > 0), + handle_event< event_handle_type::transition >, + typename ::std::conditional< + (::psst::meta::find_if< + def::handles_event::template type, + typename FSM::internal_transitions >::type::size > 0), + handle_event< event_handle_type::internal_transition >, + handle_event< event_handle_type::inner_state > + >::type + >::type{}; + +template +struct transits_on_event + : ::std::conditional< + !def::traits::is_internal_transition::value && + ::std::is_same< typename Transition::source_state_type, SourceState >::value && + ::std::is_same< typename Transition::event_type, Event >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +template < typename State, typename FSM, typename Event > +struct has_on_exit { +private: + static FSM& fsm; + static Event const& event; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval().on_exit(event, fsm) ) const* ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template < typename State, typename FSM, typename Event > +struct has_on_enter { +private: + static FSM& fsm; + static Event& event; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval().on_enter(::std::move(event), fsm) ) const* ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template < typename Activity, typename FSM, typename State > +struct activity_has_start { +private: + static FSM& fsm; + static State& state; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval().start(fsm, state) ) const* ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template < typename Activity, typename FSM, typename State > +struct activity_has_stop { +private: + static FSM& fsm; + static State& state; + + template < typename U > + static ::std::true_type + test( decltype( ::std::declval().stop(fsm, state) ) const* ); + + template < typename U > + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(nullptr) )::value; +}; + +template < typename Activity, typename FSM, typename State > +struct is_valid_activity { + static constexpr bool value = activity_has_start::value + && activity_has_stop::value; +}; + +template < typename FSM, typename State > +struct is_valid_activity < void, FSM, State >{ + static constexpr bool value = true; +}; + +template < typename FSM, typename State, typename Event, bool hasExit > +struct state_exit_impl { + void + operator()(State& state, Event const& event, FSM& fsm) const + { + state.state_exit(event, fsm); + state.on_exit(event, fsm); + } +}; + +template < typename FSM, typename State, typename Event > +struct state_exit_impl< FSM, State, Event, false > { + void + operator()(State& state, Event const& event, FSM& fsm) const + { + state.state_exit(event, fsm); + } +}; + +// TODO Parameter to allow/disallow empty on_exit function +template < typename FSM, typename State, typename Event > +struct state_exit : state_exit_impl< FSM, State, Event, + has_on_exit::value > {}; + +template < typename FSM, typename State, bool hasExit > +struct state_enter_impl { + template < typename Event > + void + operator()(State& state, Event&& event, FSM& fsm) const + { + state.on_enter(::std::forward(event), fsm); + state.state_enter(::std::forward(event), fsm); + } +}; + +template < typename FSM, typename State > +struct state_enter_impl< FSM, State, false > { + template < typename Event > + void + operator()(State& state, Event&& event, FSM& fsm) const + { + state.state_enter(::std::forward(event), fsm); + } +}; + +// TODO Parameter to allow/disallow empty enter function +template < typename FSM, typename State, typename Event > +struct state_enter : state_enter_impl::value> {}; + +template < typename FSM, typename State, bool HasHistory > +struct state_clear_impl { + bool + operator()(FSM& fsm, State& state) const + { + state = State{fsm}; + return true; + } +}; + +template < typename FSM, typename State > +struct state_clear_impl< FSM, State, true > { + bool + operator()(FSM&, State&) const + { return false; } +}; + +template < typename FSM, typename State > +struct state_clear : state_clear_impl< FSM, State, + def::traits::has_history< State >::value > {}; + +template < typename FSM, typename StateTable > +struct no_transition { + template < typename Event > + actions::event_process_result + operator()(StateTable&, Event&&) const + { + return actions::event_process_result::refuse; + } +}; + +template < typename FSM, typename StateTable, typename Transition > +struct single_transition; + +template < typename FSM, typename StateTable, + typename SourceState, typename Event, typename TargetState, + typename Action, typename Guard > +struct single_transition > > { + + using fsm_type = FSM; + using state_table = StateTable; + using source_state_def = SourceState; + using target_state_def = TargetState; + + using source_state_type = typename afsm::detail::front_state_type::type; + using target_state_type = typename afsm::detail::front_state_type::type; + + using states_def = typename fsm_type::inner_states_def; + + using guard_type = actions::detail::guard_check; + using source_exit = state_exit; + using target_enter = state_enter; + using action_type = actions::detail::action_invocation; + using state_clear_type = state_clear; + + using source_index = ::psst::meta::index_of; + using target_index = ::psst::meta::index_of; + + static_assert(source_index::found, "Failed to find source state index"); + static_assert(target_index::found, "Failed to find target state index"); + + template < typename Evt > + actions::event_process_result + operator()(state_table& states, Evt&& event) const + { + return states.template transit_state< source_state_def, target_state_def > + ( ::std::forward(event), guard_type{}, action_type{}, + source_exit{}, target_enter{}, state_clear_type{}); + } +}; + +template < ::std::size_t N, typename FSM, typename StateTable, typename Transitions > +struct nth_transition { + static_assert(Transitions::size > N, "Transition list is too small"); + using transition = typename Transitions::template type; + using event_type = typename transition::event_type; + using previous_transition = nth_transition; + using transition_invocation = single_transition>; + + template < typename Event > + actions::event_process_result + operator()(StateTable& states, Event&& event) const + { + auto res = previous_transition{}(states, ::std::forward(event)); + if (res == actions::event_process_result::refuse) { + return transition_invocation{}(states, ::std::forward(event)); + } + return res; + } +}; + +template < typename FSM, typename StateTable, typename Transitions > +struct nth_transition< 0, FSM, StateTable, Transitions > { + static_assert(Transitions::size > 0, "Transition list is too small"); + using transition = typename Transitions::template type<0>; + using event_type = typename transition::event_type; + using transition_invocation = single_transition>; + + template < typename Event > + actions::event_process_result + operator()(StateTable& states, Event&& event) const + { + return transition_invocation{}(states, ::std::forward(event)); + } +}; + +template < typename FSM, typename StateTable, typename Transitions > +struct conditional_transition { + static constexpr ::std::size_t size = Transitions::size; + static_assert(Transitions::size > 0, "Transition list is too small"); + + using last_transition = nth_transition; + + template < typename Event > + actions::event_process_result + operator()(StateTable& states, Event&& event) const + { + return last_transition{}(states, ::std::forward(event)); + } +}; + +template < typename FSM, typename StateTable, typename Transitions > +struct transition_action_selector { + using type = typename ::std::conditional< + Transitions::size == 0, + no_transition, + typename ::std::conditional< + Transitions::size == 1, + single_transition, + conditional_transition + >::type + >::type; +}; + +template < typename T, ::std::size_t StateIndex > +struct common_base_cast_func { + static constexpr ::std::size_t state_index = StateIndex; + using type = typename ::std::decay::type; + + template < typename StateTuple > + type& + operator()(StateTuple& states) const + { + return static_cast< type& >(::std::get< state_index >(states)); + } + template < typename StateTuple > + type const& + operator()(StateTuple const& states) const + { + return static_cast< type const& >(::std::get< state_index >(states)); + } +}; + +template < ::std::size_t StateIndex > +struct final_state_exit_func { + static constexpr ::std::size_t state_index = StateIndex; + + template < typename StateTuple, typename Event, typename FSM > + void + operator()(StateTuple& states, Event&& event, FSM& fsm) + { + using final_state_type = typename ::std::tuple_element< state_index, StateTuple >::type; + using final_exit = state_exit< FSM, final_state_type, Event >; + + auto& final_state = ::std::get(states); + final_exit{}(final_state, ::std::forward(event), fsm); + } +}; + +template < ::std::size_t StateIndex > +struct get_current_events_func { + static constexpr ::std::size_t state_index = StateIndex; + + template < typename StateTuple > + ::afsm::detail::event_set + operator()(StateTuple const& states) const + { + auto const& state = ::std::get(states); + return state.current_handled_events(); + } +}; + +template < ::std::size_t StateIndex > +struct get_current_deferred_events_func { + static constexpr ::std::size_t state_index = StateIndex; + + template < typename StateTuple > + ::afsm::detail::event_set + operator()(StateTuple const& states) const + { + auto const& state = ::std::get(states); + return state.current_deferrable_events(); + } +}; + +} /* namespace detail */ + +template < typename FSM, typename FSM_DEF, typename Size > +class state_transition_table { +public: + using fsm_type = FSM; + using state_machine_definition_type = FSM_DEF; + using size_type = Size; + + using this_type = state_transition_table; + + using transitions = + typename state_machine_definition_type::transitions; + static_assert(!::std::is_same::value, + "Transition table is not defined for a state machine"); + using transitions_tuple = + typename transitions::transitions; + using initial_state = + typename state_machine_definition_type::initial_state; + using inner_states_def = + typename def::detail::inner_states< transitions >::type; + using inner_states_constructor = + afsm::detail::front_state_tuple< fsm_type, inner_states_def >; + using inner_states_tuple = + typename inner_states_constructor::type; + using dispatch_table = + actions::detail::inner_dispatch_table< inner_states_tuple >; + + static constexpr ::std::size_t initial_state_index = + ::psst::meta::index_of::value; + static constexpr ::std::size_t size = inner_states_def::size; + + using state_indexes = typename ::psst::meta::index_builder::type; + using event_set = ::afsm::detail::event_set; + + template < typename Event > + using transition_table_type = ::std::array< + ::std::function< actions::event_process_result(this_type&, Event&&) >, size >; + + template < typename Event > + using exit_table_type = ::std::array< + ::std::function< void(inner_states_tuple&, Event&&, fsm_type&) >, size >; + + using current_events_table = ::std::array< + ::std::function< event_set(inner_states_tuple const&) >, size >; + using available_transtions_table = ::std::array< event_set, size >; + + template < typename CommonBase, typename StatesTuple > + using cast_table_type = ::std::array< ::std::function< + CommonBase&( StatesTuple& ) >, size >; +public: + state_transition_table(fsm_type& fsm) + : fsm_{&fsm}, + current_state_{initial_state_index}, + states_{ inner_states_constructor::construct(fsm) } + {} + + state_transition_table(fsm_type& fsm, state_transition_table const& rhs) + : fsm_{&fsm}, + current_state_{ (::std::size_t)rhs.current_state_ }, + states_{ inner_states_constructor::copy_construct(fsm, rhs.states_) } + {} + state_transition_table(fsm_type& fsm, state_transition_table&& rhs) + : fsm_{&fsm}, + current_state_{ (::std::size_t)rhs.current_state_ }, + states_{ inner_states_constructor::move_construct(fsm, ::std::move(rhs.states_)) } + {} + + state_transition_table(state_transition_table const&) = delete; + state_transition_table(state_transition_table&& rhs) + : fsm_{rhs.fsm_}, + current_state_{ (::std::size_t)rhs.current_state_ }, + states_{ ::std::move(rhs.states_) } + {} + state_transition_table& + operator = (state_transition_table const&) = delete; + state_transition_table& + operator = (state_transition_table&&) = delete; + + void + swap(state_transition_table& rhs) + { + using ::std::swap; + swap(current_state_, rhs.current_state_); + swap(states_, rhs.states_); + set_fsm(*fsm_); + rhs.set_fsm(*rhs.fsm_); + } + + void + set_fsm(fsm_type& fsm) + { + fsm_ = &fsm; + ::afsm::detail::set_enclosing_fsm< size - 1 >::set(fsm, states_); + } + + inner_states_tuple& + states() + { return states_; } + inner_states_tuple const& + states() const + { return states_; } + + template < ::std::size_t N> + typename ::std::tuple_element< N, inner_states_tuple >::type& + get_state() + { return ::std::get(states_); } + template < ::std::size_t N> + typename ::std::tuple_element< N, inner_states_tuple >::type const& + get_state() const + { return ::std::get(states_); } + + ::std::size_t + current_state() const + { return (::std::size_t)current_state_; } + + void + set_current_state(::std::size_t val) + { current_state_ = val; } + + template < typename Event > + actions::event_process_result + process_event(Event&& event) + { + // Try dispatch to inner states + auto res = dispatch_table::process_event(states_, current_state(), + ::std::forward(event)); + if (res == actions::event_process_result::refuse) { + // Check if the event can cause a transition and process it + res = process_transition_event(::std::forward(event)); + } + if (res == actions::event_process_result::process) { + check_default_transition(); + } + return res; + } + + void + check_default_transition() + { + auto const& ttable = transition_table( state_indexes{} ); + ttable[current_state()](*this, none{}); + } + + template < typename Event > + void + enter(Event&& event) + { + using initial_state_type = typename ::std::tuple_element::type; + using initial_enter = detail::state_enter< fsm_type, initial_state_type, Event >; + + auto& initial = ::std::get< initial_state_index >(states_); + initial_enter{}(initial, ::std::forward(event), *fsm_); + check_default_transition(); + } + template < typename Event > + void + exit(Event&& event) + { + auto const& table = exit_table( state_indexes{} ); + table[current_state()](states_, ::std::forward(event), *fsm_); + } + + template < typename Event > + actions::event_process_result + process_transition_event(Event&& event) + { + auto const& inv_table = transition_table( state_indexes{} ); + return inv_table[current_state()](*this, ::std::forward(event)); + } + + template < typename SourceState, typename TargetState, + typename Event, typename Guard, typename Action, + typename SourceExit, typename TargetEnter, typename SourceClear > + actions::event_process_result + transit_state(Event&& event, Guard guard, Action action, SourceExit exit, + TargetEnter enter, SourceClear clear) + { + using source_index = ::psst::meta::index_of; + using target_index = ::psst::meta::index_of; + + static_assert(source_index::found, "Failed to find source state index"); + static_assert(target_index::found, "Failed to find target state index"); + + auto& source = ::std::get< source_index::value >(states_); + auto& target = ::std::get< target_index::value >(states_); + return transit_state_impl( + ::std::forward(event), source, target, + guard, action, exit, enter, clear, + target_index::value, + typename def::traits::exception_safety::type{}); + } + template < typename SourceState, typename TargetState, + typename Event, typename Guard, typename Action, + typename SourceExit, typename TargetEnter, typename SourceClear > + actions::event_process_result + transit_state_impl(Event&& event, SourceState& source, TargetState& target, + Guard guard, Action action, SourceExit exit, + TargetEnter enter, SourceClear clear, + ::std::size_t target_index, + def::tags::basic_exception_safety const&) + { + if (guard(*fsm_, source, event)) { + auto const& observer = root_machine(*fsm_); + exit(source, ::std::forward(event), *fsm_); + observer.state_exited(*fsm_, source, event); + action(::std::forward(event), *fsm_, source, target); + enter(target, ::std::forward(event), *fsm_); + observer.state_entered(*fsm_, target, event); + if (clear(*fsm_, source)) + observer.state_cleared(*fsm_, source); + current_state_ = target_index; + observer.state_changed(*fsm_, source, target, event); + return actions::event_process_result::process; + } + return actions::event_process_result::refuse; + } + template < typename SourceState, typename TargetState, + typename Event, typename Guard, typename Action, + typename SourceExit, typename TargetEnter, typename SourceClear > + actions::event_process_result + transit_state_impl(Event&& event, SourceState& source, TargetState& target, + Guard guard, Action action, SourceExit exit, + TargetEnter enter, SourceClear clear, + ::std::size_t target_index, + def::tags::strong_exception_safety const&) + { + SourceState source_backup{source}; + TargetState target_backup{target}; + try { + return transit_state_impl( + ::std::forward(event), source, target, + guard, action, exit, enter, clear, + target_index, + def::tags::basic_exception_safety{}); + } catch (...) { + using ::std::swap; + swap(source, source_backup); + swap(target, target_backup); + throw; + } + } + template < typename SourceState, typename TargetState, + typename Event, typename Guard, typename Action, + typename SourceExit, typename TargetEnter, typename SourceClear > + actions::event_process_result + transit_state_impl(Event&& event, SourceState& source, TargetState& target, + Guard guard, Action action, SourceExit exit, + TargetEnter enter, SourceClear clear, + ::std::size_t target_index, + def::tags::nothrow_guarantee const&) + { + SourceState source_backup{source}; + TargetState target_backup{target}; + try { + return transit_state_impl( + ::std::forward(event), source, target, + guard, action, exit, enter, clear, + target_index, + def::tags::basic_exception_safety{}); + } catch (...) {} + using ::std::swap; + swap(source, source_backup); + swap(target, target_backup); + return actions::event_process_result::refuse; + } + + event_set + current_handled_events() const + { + auto const& table = get_current_events_table(state_indexes{}); + auto res = table[current_state_](states_); + auto const& available_transitions + = get_available_transitions_table(state_indexes{}); + auto const& trans = available_transitions[current_state_]; + res.insert( trans.begin(), trans.end()); + return res; + } + + event_set + current_deferrable_events() const + { + auto const& table = get_current_deferred_events_table(state_indexes{}); + return table[current_state_](states_); + } + + template < typename T > + T& + cast_current_state() + { + using target_type = typename ::std::decay::type; + auto const& ct = get_cast_table( state_indexes{} ); + return ct[current_state_]( states_ ); + } + template < typename T > + T const& + cast_current_state() const + { + using target_type = typename ::std::add_const< typename ::std::decay::type >::type; + auto const& ct = get_cast_table( state_indexes{} ); + return ct[current_state_]( states_ ); + } +private: + template < typename Event, ::std::size_t ... Indexes > + static transition_table_type< Event > const& + transition_table( ::psst::meta::indexes_tuple< Indexes... > const& ) + { + using event_type = typename ::std::decay::type; + using event_transitions = typename ::psst::meta::find_if< + def::handles_event< event_type >::template type, transitions_tuple >::type; + static transition_table_type< Event > _table {{ + typename detail::transition_action_selector< fsm_type, this_type, + typename ::psst::meta::find_if< + def::originates_from< + typename inner_states_def::template type< Indexes > + >::template type, + event_transitions + >::type >::type{} ... + }}; + return _table; + } + template < typename Event, ::std::size_t ... Indexes > + static exit_table_type const& + exit_table( ::psst::meta::indexes_tuple< Indexes... > const& ) + { + static exit_table_type _table {{ + detail::final_state_exit_func{} ... + }}; + return _table; + } + template < ::std::size_t ... Indexes > + static current_events_table const& + get_current_events_table( ::psst::meta::indexes_tuple< Indexes ... > const& ) + { + static current_events_table _table{{ + detail::get_current_events_func{} ... + }}; + + return _table; + } + template < ::std::size_t ... Indexes > + static current_events_table const& + get_current_deferred_events_table( ::psst::meta::indexes_tuple< Indexes ... > const& ) + { + static current_events_table _table{{ + detail::get_current_deferred_events_func{} ... + }}; + + return _table; + } + template < ::std::size_t ... Indexes > + static available_transtions_table const& + get_available_transitions_table( ::psst::meta::indexes_tuple< Indexes ...> const& ) + { + static available_transtions_table _table{{ + ::afsm::detail::make_event_set( + typename ::psst::meta::transform< + def::detail::event_type, + typename ::psst::meta::find_if< + def::originates_from< + typename inner_states_def::template type< Indexes > + >:: template type, + transitions_tuple + >::type + > ::type {} + ) ... + }}; + + return _table; + } + template < typename T, typename StateTuple, ::std::size_t ... Indexes > + static cast_table_type const& + get_cast_table( ::psst::meta::indexes_tuple< Indexes... > const& ) + { + static cast_table_type _table {{ + detail::common_base_cast_func{}... + }}; + return _table; + } +private: + fsm_type* fsm_; + size_type current_state_; + inner_states_tuple states_; +}; + +template < typename FSM, typename FSM_DEF, typename Size > +class state_transition_stack { +public: + using state_table_type = state_transition_table; + using this_type = state_transition_stack; + + using fsm_type = typename state_table_type::fsm_type; + using state_machine_definition_type = typename state_table_type::state_machine_definition_type; + using size_type = typename state_table_type::size_type; + using inner_states_tuple = typename state_table_type::inner_states_tuple; + using event_set = typename state_table_type::event_set; + + using stack_constructor_type = afsm::detail::stack_constructor; +public: + state_transition_stack(fsm_type& fsm) + : fsm_{&fsm}, + state_stack_{ stack_constructor_type::construct(fsm) } + {} + state_transition_stack(fsm_type& fsm, state_transition_stack const& rhs) + : fsm_{&fsm}, + state_stack_{ stack_constructor_type::copy_construct(fsm, rhs.state_stack_) } + {} + state_transition_stack(fsm_type& fsm, state_transition_stack&& rhs) + : fsm_{&fsm}, + state_stack_{ stack_constructor_type::move_construct(fsm, ::std::move(rhs.state_stack_)) } + {} + + state_transition_stack(state_transition_stack const&) = delete; + state_transition_stack(state_transition_stack&&) = delete; + state_transition_stack& + operator = (state_transition_stack const&) = delete; + state_transition_stack& + operator = (state_transition_stack&&) = delete; + + void + swap(state_transition_stack& rhs) + { + using ::std::swap; + swap(state_stack_, rhs.state_stack_); + set_fsm(*fsm_); + rhs.set_fsm(*rhs.fsm_); + } + + void + set_fsm(fsm_type& fsm) + { + fsm_ = &fsm; + for (auto& item : state_stack_) { + item.set_fsm(fsm); + } + } + + inner_states_tuple& + states() + { return top().states(); } + inner_states_tuple const& + states() const + { return top().states(); } + + template < ::std::size_t N > + typename ::std::tuple_element< N, inner_states_tuple >::type& + get_state() + { return top().template get_state(); } + template < ::std::size_t N > + typename ::std::tuple_element< N, inner_states_tuple >::type const& + get_state() const + { return top().template get_state(); } + + ::std::size_t + current_state() const + { return top().current_state(); } + + template < typename Event > + actions::event_process_result + process_event(Event&& event) + { + return top().process_event(::std::forward(event)); + } + + template < typename Event > + void + enter(Event&& event) + { + top().enter(::std::forward(event)); + } + template < typename Event > + void + exit(Event&& event) + { + top().exit(::std::forward(event)); + } + + template < typename Event > + void + push(Event&& event) + { + state_stack_.emplace_back( *fsm_ ); + enter(::std::forward(event)); + } + + template < typename Event > + void + pop(Event&& event) + { + if (state_stack_.size() > 1) { + exit(::std::forward(event)); + state_stack_.pop_back(); + } + } + + ::std::size_t + stack_size() const + { + return state_stack_.size(); + } + + event_set + current_handled_events() const + { + return top().current_handled_events(); + } + event_set + current_deferrable_events() const + { + return top().current_deferrable_events(); + } + + template < typename T > + T& + cast_current_state() + { + return top().template cast_current_state(); + } + template < typename T > + T const& + cast_current_state() const + { + return top().template cast_current_state(); + } +private: + using stack_type = typename stack_constructor_type::type; + + state_table_type& + top() + { return state_stack_.back(); } + state_table_type const& + top() const + { return state_stack_.back(); } +private: + fsm_type* fsm_; + stack_type state_stack_; +}; + +template < typename FSM, typename FSM_DEF, typename Size, bool HasPusdowns > +struct transition_container_selector { + using type = state_transition_table; +}; + +template +struct transition_container_selector { + using type = state_transition_stack; +}; + +namespace detail { + +template < typename FSM, typename FSM_DEF, typename Size, bool HasPushdowns > +struct transition_container { + using transitions_tuple = typename transition_container_selector::type; + + transition_container(FSM* fsm) + : transitions_{*fsm} + {} + transition_container(FSM* fsm, transition_container const& rhs) + : transitions_{*fsm, rhs.transitions_} + {} + transition_container(FSM* fsm, transition_container&& rhs) + : transitions_{*fsm, ::std::move(rhs.transitions_)} + {} +protected: + transitions_tuple transitions_; +}; + +template < typename FSM, typename FSM_DEF, typename Size > +struct transition_container< FSM, FSM_DEF, Size, true > { + using transitions_tuple = typename transition_container_selector::type; + + transition_container(FSM* fsm) + : transitions_{*fsm} + {} + transition_container(FSM* fsm, transition_container const& rhs) + : transitions_{*fsm, rhs.transitions_} + {} + transition_container(FSM* fsm, transition_container&& rhs) + : transitions_{*fsm, ::std::move(rhs.transitions_)} + {} + + ::std::size_t + stack_size() const + { + return transitions_.stack_size(); + } + +protected: + template < typename T > + friend struct ::afsm::detail::pushdown_state; + template < typename T > + friend struct ::afsm::detail::popup_state; + // Push/pop ops + template < typename Event > + void + pushdown(Event&& event) + { + transitions_.push(::std::forward(event)); + } + template < typename Event > + void + popup(Event&& event) + { + transitions_.pop(::std::forward(event)); + } +protected: + transitions_tuple transitions_; +}; +} /* namespace detail */ + +template < typename FSM, typename FSM_DEF, typename Size > +struct transition_container + : detail::transition_container::value> { + using base_type = detail::transition_container::value>; + using transitions_tuple = typename base_type::transitions_tuple; + + transition_container( FSM* fsm ) : base_type{fsm} {} + transition_container(FSM* fsm, transition_container const& rhs) + : base_type{fsm, rhs} {} + transition_container(FSM* fsm, transition_container&& rhs) + : base_type{fsm, ::std::move(rhs)} {} +protected: + using base_type::transitions_; +}; + +} /* namespace transitions */ +} /* namespace afsm */ + +#endif /* AFSM_DETAIL_TRANSITIONS_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/fsm.hpp b/src/systems/elevator/vender/afsm/include/afsm/fsm.hpp new file mode 100644 index 0000000000..9f22482cc6 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/fsm.hpp @@ -0,0 +1,777 @@ +/* + * fsm.hpp + * + * Created on: 25 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_FSM_HPP_ +#define AFSM_FSM_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +namespace afsm { + +//---------------------------------------------------------------------------- +// State +//---------------------------------------------------------------------------- +template < typename T, typename FSM > +class state : public detail::state_base< T > { +public: + using enclosing_fsm_type = FSM; +public: + state(enclosing_fsm_type& fsm) + : state::state_type{}, fsm_{&fsm} + {} + state(state const& rhs) = default; + state(state&& rhs) = default; + state(enclosing_fsm_type& fsm, state const& rhs) + : state::state_type{static_cast(rhs)}, fsm_{&fsm} + {} + state(enclosing_fsm_type& fsm, state&& rhs) + : state::state_type{static_cast(rhs)}, fsm_{&fsm} + {} + + state& + operator = (state const& rhs) + { + state{rhs}.swap(*this); + return *this; + } + state& + operator = (state&& rhs) + { + swap(rhs); + return *this; + } + + void + swap(state& rhs) noexcept + { + static_cast(*this).swap(rhs); + } + + enclosing_fsm_type& + enclosing_fsm() + { return *fsm_; } + enclosing_fsm_type const& + enclosing_fsm() const + { return *fsm_; } + void + enclosing_fsm(enclosing_fsm_type& fsm) + { fsm_ = &fsm; } +protected: // For tests + template < ::std::size_t StateIndex > + friend struct actions::detail::process_event_handler; + + template < typename Event > + actions::event_process_result + process_event( Event&& evt ) + { + return process_event_impl(::std::forward(evt), + detail::event_process_selector< + Event, + typename state::internal_events, + typename state::deferred_events>{} ); + } +private: + template < typename Event > + actions::event_process_result + process_event_impl(Event&& evt, + detail::process_type const&) + { + return actions::handle_in_state_event(::std::forward(evt), *fsm_, *this); + } + template < typename Event > + constexpr actions::event_process_result + process_event_impl(Event&&, + detail::process_type const&) const + { + return actions::event_process_result::defer; + } + template < typename Event > + constexpr actions::event_process_result + process_event_impl(Event&&, + detail::process_type const&) const + { + return actions::event_process_result::refuse; + } +private: + enclosing_fsm_type* fsm_; +}; + +//---------------------------------------------------------------------------- +// Inner state machine +//---------------------------------------------------------------------------- +template < typename T, typename FSM > +class inner_state_machine : public detail::state_machine_base< T, none, inner_state_machine > { +public: + using enclosing_fsm_type = FSM; + using this_type = inner_state_machine; + using base_machine_type = detail::state_machine_base< T, none, this_type >; +public: + inner_state_machine(enclosing_fsm_type& fsm) + : base_machine_type{this}, fsm_{&fsm} {} + inner_state_machine(inner_state_machine const& rhs) + : base_machine_type{this, rhs}, fsm_{rhs.fsm_} {} + inner_state_machine(inner_state_machine&& rhs) + : base_machine_type{this, ::std::move(rhs)}, + fsm_{rhs.fsm_} + { + } + + inner_state_machine(enclosing_fsm_type& fsm, inner_state_machine const& rhs) + : base_machine_type{static_cast(rhs)}, fsm_{&fsm} {} + inner_state_machine(enclosing_fsm_type& fsm, inner_state_machine&& rhs) + : base_machine_type{static_cast(rhs)}, fsm_{&fsm} {} + + void + swap(inner_state_machine& rhs) noexcept + { + using ::std::swap; + static_cast(*this).swap(rhs); + swap(fsm_, rhs.fsm_); + } + inner_state_machine& + operator = (inner_state_machine const& rhs) + { + inner_state_machine tmp{rhs}; + swap(tmp); + return *this; + } + inner_state_machine& + operator = (inner_state_machine&& rhs) + { + swap(rhs); + return *this; + } + + enclosing_fsm_type& + enclosing_fsm() + { return *fsm_; } + enclosing_fsm_type const& + enclosing_fsm() const + { return *fsm_; } + void + enclosing_fsm(enclosing_fsm_type& fsm) + { fsm_ = &fsm; } +protected: // For tests + template < ::std::size_t StateIndex > + friend struct actions::detail::process_event_handler; + + template < typename Event > + actions::event_process_result + process_event( Event&& event ) + { + return process_event_impl(*fsm_, ::std::forward(event), + detail::event_process_selector< + Event, + typename inner_state_machine::handled_events, + typename inner_state_machine::deferred_events>{} ); + } +private: + using base_machine_type::process_event_impl; +private: + enclosing_fsm_type* fsm_; +}; + +//---------------------------------------------------------------------------- +// State machine +//---------------------------------------------------------------------------- +template < typename T, typename Mutex, typename Observer, + template class ObserverWrapper > +class state_machine : + public detail::state_machine_base< T, Mutex, + state_machine >, + public ObserverWrapper { +public: + static_assert( ::psst::meta::is_empty< typename T::deferred_events >::value, + "Outer state machine cannot defer events" ); + using this_type = state_machine; + using base_machine_type = detail::state_machine_base< T, Mutex, this_type >; + using mutex_type = Mutex; + using lock_guard = typename detail::lock_guard_type::type; + using observer_wrapper = ObserverWrapper; + using event_invokation = ::std::function< actions::event_process_result() >; + using event_queue_item = ::std::pair< event_invokation, detail::event_base::id_type const* >; + using event_queue = ::std::deque< event_queue_item >; + using deferred_queue = ::std::list< event_queue_item >; +public: + state_machine() + : base_machine_type{this}, + is_top_{}, + handled_{ base_machine_type::current_handled_events() }, + deferred_{ base_machine_type::current_deferrable_events() }, + mutex_{}, + queued_events_{}, + queue_size_{0}, + deferred_top_{}, + deferred_events_{}, + deferred_event_ids_{} + {} + template + explicit + state_machine(Args&& ... args) + : base_machine_type(this, ::std::forward(args)...), + is_top_{}, + handled_{ base_machine_type::current_handled_events() }, + deferred_{ base_machine_type::current_deferrable_events() }, + mutex_{}, + queued_events_{}, + queue_size_{0}, + deferred_top_{}, + deferred_events_{}, + deferred_event_ids_{} + {} + + template < typename Event > + actions::event_process_result + process_event( Event&& event ) + { + if (!queue_size_ && !is_top_.test_and_set()) { + auto res = process_event_dispatch(::std::forward(event)); + is_top_.clear(); + // Process enqueued events + process_event_queue(); + return res; + } else { + // Enqueue event + enqueue_event(::std::forward(event)); + return actions::event_process_result::defer; + } + } + + detail::event_set const& + current_handled_events() const + { return handled_; } + detail::event_set const& + current_deferrable_events() const + { return deferred_; } + detail::event_set const& + current_deferred_events() const + { return deferred_event_ids_; } + + void + clear_deferred_events() + { + lock_guard lock{mutex_}; + deferred_queue{}.swap(deferred_events_); + detail::event_set{}.swap(deferred_event_ids_); + } +private: + template < typename Event > + actions::event_process_result + process_event_dispatch( Event&& event ) + { + return process_event_impl(::std::forward(event), + detail::event_process_selector< + Event, + typename state_machine::handled_events>{} ); + } + + template < typename Event > + actions::event_process_result + process_event_impl(Event&& event, + detail::process_type const& sel) + { + using actions::event_process_result; + observer_wrapper::start_process_event(*this, ::std::forward(event)); + auto res = base_machine_type::process_event_impl(*this, ::std::forward(event), sel ); + switch (res) { + case event_process_result::process: + // Changed state. Process deferred events + handled_ = base_machine_type::current_handled_events(); + deferred_ = base_machine_type::current_deferrable_events(); + process_deferred_queue(); + break; + case event_process_result::process_in_state: + observer_wrapper::processed_in_state(*this, ::std::forward(event)); + break; + case event_process_result::defer: + // Add event to deferred queue + defer_event(::std::forward(event)); + break; + case event_process_result::refuse: + // The event cannot be processed in current state + observer_wrapper::reject_event(*this, ::std::forward(event)); + return reject_event_impl(::std::forward(event), + ::std::integral_constant::value>{}); + default: + break; + } + return res; + } + template < typename Event > + actions::event_process_result + process_event_impl(Event&&, + detail::process_type const&) + { + static_assert( detail::event_process_selector< + Event, + typename state_machine::handled_events, + typename state_machine::deferred_events>::value + != actions::event_process_result::refuse, + "Event type is not handled by this state machine" ); + return actions::event_process_result::refuse; + } + + template < typename Event > + actions::event_process_result + reject_event_impl(Event&& event, ::std::true_type const&) + { + return this->T::reject_event(::std::forward(event), *this); + } + + template < typename Event > + actions::event_process_result + reject_event_impl(Event&&, ::std::false_type const&) + { + return actions::event_process_result::refuse; + } + + template < typename Event > + void + enqueue_event(Event&& event) + { + using evt_identity = typename detail::event_identity::type; + { + lock_guard lock{mutex_}; + ++queue_size_; + observer_wrapper::enqueue_event(*this, ::std::forward(event)); + Event evt{::std::forward(event)}; + queued_events_.emplace_back([&, evt]() mutable { + return process_event_dispatch(::std::move(evt)); + }, &evt_identity::id); + } + // Process enqueued events in case we've been waiting for queue + // mutex release + process_event_queue(); + } + + void + lock_and_swap_queue(event_queue& queue) + { + lock_guard lock{mutex_}; + ::std::swap(queued_events_, queue); + queue_size_ -= queue.size(); + } + + void + process_event_queue() + { + while (queue_size_ > 0 && !is_top_.test_and_set()) { + observer_wrapper::start_process_events_queue(*this); + while (queue_size_ > 0) { + event_queue postponed; + lock_and_swap_queue(postponed); + for (auto const& event : postponed) { + event.first(); + } + } + observer_wrapper::end_process_events_queue(*this); + is_top_.clear(); + } + } + + template < typename Event > + void + defer_event(Event&& event) + { + using evt_identity = typename detail::event_identity::type; + + observer_wrapper::defer_event(*this, ::std::forward(event)); + Event evt{::std::forward(event)}; + deferred_events_.emplace_back([&, evt]() mutable { + return process_event_dispatch(::std::move(evt)); + }, &evt_identity::id); + deferred_event_ids_.insert(&evt_identity::id); + } + void + process_deferred_queue() + { + if (!deferred_top_.test_and_set()) { + using actions::event_process_result; + deferred_queue deferred; + detail::event_set event_ids; + if (skip_deferred_queue()) { + observer_wrapper::skip_processing_deferred_queue(*this); + } else { + ::std::swap(deferred_events_, deferred); + ::std::swap(deferred_event_ids_, event_ids); + } + while (!deferred.empty()) { + observer_wrapper::start_process_deferred_queue(*this, deferred.size()); + auto res = event_process_result::refuse; + for (auto event = deferred.begin(); event != deferred.end();) { + if (handled_.count(event->second)) { + res = event->first(); + deferred.erase(event++); + } else if (deferred_.count(event->second)) { + // Move directly to the deferred queue + ::std::size_t count{0}; + auto next = event; + while (next != deferred.end() && next->second == event->second) { + ++next; + ++count; + } + deferred_events_.splice(deferred_events_.end(), + deferred, event, next); + deferred_event_ids_.insert(event->second); + event = next; + observer_wrapper::postpone_deferred_events(*this, count); + } else { + deferred.erase(event++); + observer_wrapper::drop_deferred_event(*this); + } + if (res == event_process_result::process) + break; + } + for (auto event = deferred.begin(); event != deferred.end();) { + ::std::size_t count{0}; + auto next = event; + while (next != deferred.end() && next->second == event->second) { + ++count; + ++next; + } + deferred_events_.splice(deferred_events_.end(), + deferred, event, next); + deferred_event_ids_.insert(event->second); + event = next; + observer_wrapper::postpone_deferred_events(*this, count); + } + event_ids.clear(); + observer_wrapper::end_process_deferred_queue(*this, deferred_events_.size()); + if (res == event_process_result::process) { + if (skip_deferred_queue()) { + observer_wrapper::skip_processing_deferred_queue(*this); + } else { + ::std::swap(deferred_events_, deferred); + ::std::swap(deferred_event_ids_, event_ids); + } + } + } + deferred_top_.clear(); + } + } + + bool + skip_deferred_queue() const + { + detail::event_set st; + ::std::set_intersection(handled_.begin(), handled_.end(), + deferred_event_ids_.begin(), deferred_event_ids_.end(), + ::std::inserter(st, st.end())); + return st.empty(); + } +private: + using atomic_counter = ::std::atomic< ::std::size_t >; + + ::std::atomic_flag is_top_; + + detail::event_set handled_; + detail::event_set deferred_; + + mutex_type mutex_; + event_queue queued_events_; + atomic_counter queue_size_; + + ::std::atomic_flag deferred_top_; + deferred_queue deferred_events_; + detail::event_set deferred_event_ids_; +}; + +//---------------------------------------------------------------------------- +// Priority State machine +//---------------------------------------------------------------------------- +using event_priority_type = ::std::int32_t; +template < typename T > +struct event_priority_traits : ::std::integral_constant{}; + +template < typename T, typename Mutex, typename Observer, + template class ObserverWrapper > +class priority_state_machine : + public detail::state_machine_base< T, Mutex, + priority_state_machine >, + public ObserverWrapper { +public: + static_assert( ::psst::meta::is_empty< typename T::deferred_events >::value, + "Outer state machine cannot defer events" ); + using this_type = priority_state_machine; + using base_machine_type = detail::state_machine_base< T, Mutex, this_type >; + using mutex_type = Mutex; + using lock_guard = typename detail::lock_guard_type::type; + using observer_wrapper = ObserverWrapper; + using event_invokation = ::std::function< actions::event_process_result() >; + using prioritized_event = ::std::pair; + struct event_comparison { + bool + operator()(prioritized_event const& lhs, prioritized_event const& rhs) const + { return lhs.second < rhs.second; } + }; + using event_queue = ::std::priority_queue< prioritized_event, ::std::vector, event_comparison >; +public: + priority_state_machine() + : base_machine_type{this}, + is_top_{}, + mutex_{}, + queued_events_{}, + queue_size_{0}, + deferred_mutex_{}, + deferred_events_{} + {} + template + explicit + priority_state_machine(Args&& ... args) + : base_machine_type(this, ::std::forward(args)...), + is_top_{}, + mutex_{}, + queued_events_{}, + queue_size_{0}, + deferred_mutex_{}, + deferred_events_{} + {} + + template < typename Event > + actions::event_process_result + process_event( Event&& event, event_priority_type priority = + event_priority_traits< typename ::std::decay::type >::value ) + { + if (!queue_size_ && !is_top_.test_and_set()) { + auto res = process_event_dispatch(::std::forward(event), priority); + is_top_.clear(); + // Process enqueued events + process_event_queue(); + return res; + } else { + // Enqueue event + enqueue_event(::std::forward(event), priority); + return actions::event_process_result::defer; + } + } +private: + template < typename Event > + actions::event_process_result + process_event_dispatch( Event&& event, event_priority_type priority ) + { + return process_event_impl(::std::forward(event), priority, + detail::event_process_selector< + Event, + typename priority_state_machine::handled_events>{} ); + } + + template < typename Event > + actions::event_process_result + process_event_impl(Event&& event, event_priority_type priority, + detail::process_type const& sel) + { + using actions::event_process_result; + observer_wrapper::start_process_event(*this, ::std::forward(event)); + auto res = base_machine_type::process_event_impl(*this, ::std::forward(event), sel ); + switch (res) { + case event_process_result::process: + // Changed state. Process deferred events + process_deferred_queue(); + break; + case event_process_result::process_in_state: + observer_wrapper::processed_in_state(*this, ::std::forward(event)); + break; + case event_process_result::defer: + // Add event to deferred queue + defer_event(::std::forward(event), priority); + break; + case event_process_result::refuse: + // The event cannot be processed in current state + observer_wrapper::reject_event(*this, ::std::forward(event)); + break; + default: + break; + } + return res; + } + template < typename Event > + actions::event_process_result + process_event_impl(Event&&, event_priority_type, + detail::process_type const&) + { + static_assert( detail::event_process_selector< + Event, + typename priority_state_machine::handled_events, + typename priority_state_machine::deferred_events>::value + != actions::event_process_result::refuse, + "Event type is not handled by this state machine" ); + return actions::event_process_result::refuse; + } + + template < typename Event > + void + enqueue_event(Event&& event, event_priority_type priority) + { + { + lock_guard lock{mutex_}; + ++queue_size_; + observer_wrapper::enqueue_event(*this, ::std::forward(event)); + Event evt{::std::forward(event)}; + queued_events_.emplace([&, evt, priority]() mutable { + return process_event_dispatch(::std::move(evt), priority); + }, priority); + } + // Process enqueued events in case we've been waiting for queue + // mutex release + process_event_queue(); + } + + void + lock_and_swap_queue(event_queue& queue) + { + lock_guard lock{mutex_}; + ::std::swap(queued_events_, queue); + queue_size_ -= queue.size(); + } + + void + process_event_queue() + { + while (queue_size_ > 0 && !is_top_.test_and_set()) { + observer_wrapper::start_process_events_queue(*this); + while (queue_size_ > 0) { + event_queue postponed; + lock_and_swap_queue(postponed); + while (!postponed.empty()) { + postponed.top().first(); + postponed.pop(); + } + } + observer_wrapper::end_process_events_queue(*this); + is_top_.clear(); + } + } + + template < typename Event > + void + defer_event(Event&& event, event_priority_type priority) + { + lock_guard lock{deferred_mutex_}; + observer_wrapper::defer_event(*this, ::std::forward(event)); + Event evt{::std::forward(event)}; + deferred_events_.emplace([&, evt, priority]() mutable { + return process_event_dispatch(::std::move(evt), priority); + }, priority); + } + void + process_deferred_queue() + { + using actions::event_process_result; + event_queue deferred; + { + lock_guard lock{deferred_mutex_}; + ::std::swap(deferred_events_, deferred); + } + while (!deferred.empty()) { + observer_wrapper::start_process_deferred_queue(*this, deferred.size()); + auto res = event_process_result::refuse; + while (!deferred.empty()) { + res = deferred.top().first(); + deferred.pop(); + if (res == event_process_result::process) + break; + } + { + lock_guard lock{deferred_mutex_}; + while (!deferred.empty()) { + deferred_events_.push(deferred.top()); + deferred.pop(); + } + } + observer_wrapper::end_process_deferred_queue(*this, deferred_events_.size()); + if (res == event_process_result::process) { + ::std::swap(deferred_events_, deferred); + } + } + } +private: + using atomic_counter = ::std::atomic< ::std::size_t >; + + ::std::atomic_flag is_top_; + + mutex_type mutex_; + event_queue queued_events_; + atomic_counter queue_size_; + + mutex_type deferred_mutex_; + event_queue deferred_events_; +}; + +template < typename T, typename Mutex, typename Observer, + template class ObserverWrapper > +state_machine& +root_machine(state_machine& fsm) +{ + return fsm; +} + +template < typename T, typename Mutex, typename Observer, + template class ObserverWrapper > +state_machine const& +root_machine(state_machine const& fsm) +{ + return fsm; +} + +template < typename T, typename Mutex, typename Observer, + template class ObserverWrapper > +priority_state_machine& +root_machine(priority_state_machine& fsm) +{ + return fsm; +} + +template < typename T, typename Mutex, typename Observer, + template class ObserverWrapper > +priority_state_machine const& +root_machine(priority_state_machine const& fsm) +{ + return fsm; +} + +template < typename T, typename FSM > +auto +root_machine(inner_state_machine& fsm) + -> decltype(root_machine(fsm.enclosing_fsm())) +{ + return root_machine(fsm.enclosing_fsm()); +} + +template < typename T, typename FSM > +auto +root_machine(inner_state_machine const& fsm) + -> decltype(root_machine(fsm.enclosing_fsm())) +{ + return root_machine(fsm.enclosing_fsm()); +} + +template < typename T, typename FSM > +auto +root_machine(state& fsm) + -> decltype(root_machine(fsm.enclosing_fsm())) +{ + return root_machine(fsm.enclosing_fsm()); +} + +template < typename T, typename FSM > +auto +root_machine(state const& fsm) + -> decltype(root_machine(fsm.enclosing_fsm())) +{ + return root_machine(fsm.enclosing_fsm()); +} + +} /* namespace afsm */ + +#endif /* AFSM_FSM_HPP_ */ diff --git a/src/systems/elevator/vender/afsm/include/afsm/fsm_fwd.hpp b/src/systems/elevator/vender/afsm/include/afsm/fsm_fwd.hpp new file mode 100644 index 0000000000..b34c919769 --- /dev/null +++ b/src/systems/elevator/vender/afsm/include/afsm/fsm_fwd.hpp @@ -0,0 +1,34 @@ +/* + * fsm_fwd.hpp + * + * Created on: 28 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef AFSM_FSM_FWD_HPP_ +#define AFSM_FSM_FWD_HPP_ + +namespace afsm { + +struct none {}; + +namespace detail { +template < typename Observer > +class observer_wrapper; + +} + +template < typename FSM, typename T > +class state; +template < typename FSM, typename T > +class inner_state_machine; +template < typename T, typename Mutex = none, typename Observer = none, + template class ObserverWrapper = detail::observer_wrapper > +class state_machine; +template < typename T, typename Mutex = none, typename Observer = none, + template class ObserverWrapper = detail::observer_wrapper > +class priority_state_machine; + +} /* namespace afsm */ + +#endif /* AFSM_FSM_FWD_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/LICENSE b/src/systems/elevator/vender/metapushkin/LICENSE new file mode 100644 index 0000000000..6a3a57fb25 --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/LICENSE @@ -0,0 +1,201 @@ + The Artistic License 2.0 + + Copyright (c) 2000-2006, The Perl Foundation. + + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +Preamble + +This license establishes the terms under which a given free software +Package may be copied, modified, distributed, and/or redistributed. +The intent is that the Copyright Holder maintains some artistic +control over the development of that Package while still keeping the +Package available as open source and free software. + +You are always permitted to make arrangements wholly outside of this +license directly with the Copyright Holder of a given Package. If the +terms of this license do not permit the full use that you propose to +make of the Package, you should contact the Copyright Holder and seek +a different licensing arrangement. + +Definitions + + "Copyright Holder" means the individual(s) or organization(s) + named in the copyright notice for the entire Package. + + "Contributor" means any party that has contributed code or other + material to the Package, in accordance with the Copyright Holder's + procedures. + + "You" and "your" means any person who would like to copy, + distribute, or modify the Package. + + "Package" means the collection of files distributed by the + Copyright Holder, and derivatives of that collection and/or of + those files. A given Package may consist of either the Standard + Version, or a Modified Version. + + "Distribute" means providing a copy of the Package or making it + accessible to anyone else, or in the case of a company or + organization, to others outside of your company or organization. + + "Distributor Fee" means any fee that you charge for Distributing + this Package or providing support for this Package to another + party. It does not mean licensing fees. + + "Standard Version" refers to the Package if it has not been + modified, or has been modified only in ways explicitly requested + by the Copyright Holder. + + "Modified Version" means the Package, if it has been changed, and + such changes were not explicitly requested by the Copyright + Holder. + + "Original License" means this Artistic License as Distributed with + the Standard Version of the Package, in its current version or as + it may be modified by The Perl Foundation in the future. + + "Source" form means the source code, documentation source, and + configuration files for the Package. + + "Compiled" form means the compiled bytecode, object code, binary, + or any other form resulting from mechanical transformation or + translation of the Source form. + + +Permission for Use and Modification Without Distribution + +(1) You are permitted to use the Standard Version and create and use +Modified Versions for any purpose without restriction, provided that +you do not Distribute the Modified Version. + + +Permissions for Redistribution of the Standard Version + +(2) You may Distribute verbatim copies of the Source form of the +Standard Version of this Package in any medium without restriction, +either gratis or for a Distributor Fee, provided that you duplicate +all of the original copyright notices and associated disclaimers. At +your discretion, such verbatim copies may or may not include a +Compiled form of the Package. + +(3) You may apply any bug fixes, portability changes, and other +modifications made available from the Copyright Holder. The resulting +Package will still be considered the Standard Version, and as such +will be subject to the Original License. + + +Distribution of Modified Versions of the Package as Source + +(4) You may Distribute your Modified Version as Source (either gratis +or for a Distributor Fee, and with or without a Compiled form of the +Modified Version) provided that you clearly document how it differs +from the Standard Version, including, but not limited to, documenting +any non-standard features, executables, or modules, and provided that +you do at least ONE of the following: + + (a) make the Modified Version available to the Copyright Holder + of the Standard Version, under the Original License, so that the + Copyright Holder may include your modifications in the Standard + Version. + + (b) ensure that installation of your Modified Version does not + prevent the user installing or running the Standard Version. In + addition, the Modified Version must bear a name that is different + from the name of the Standard Version. + + (c) allow anyone who receives a copy of the Modified Version to + make the Source form of the Modified Version available to others + under + + (i) the Original License or + + (ii) a license that permits the licensee to freely copy, + modify and redistribute the Modified Version using the same + licensing terms that apply to the copy that the licensee + received, and requires that the Source form of the Modified + Version, and of any works derived from it, be made freely + available in that license fees are prohibited but Distributor + Fees are allowed. + + +Distribution of Compiled Forms of the Standard Version +or Modified Versions without the Source + +(5) You may Distribute Compiled forms of the Standard Version without +the Source, provided that you include complete instructions on how to +get the Source of the Standard Version. Such instructions must be +valid at the time of your distribution. If these instructions, at any +time while you are carrying out such distribution, become invalid, you +must provide new instructions on demand or cease further distribution. +If you provide valid instructions or cease distribution within thirty +days after you become aware that the instructions are invalid, then +you do not forfeit any of your rights under this license. + +(6) You may Distribute a Modified Version in Compiled form without +the Source, provided that you comply with Section 4 with respect to +the Source of the Modified Version. + + +Aggregating or Linking the Package + +(7) You may aggregate the Package (either the Standard Version or +Modified Version) with other packages and Distribute the resulting +aggregation provided that you do not charge a licensing fee for the +Package. Distributor Fees are permitted, and licensing fees for other +components in the aggregation are permitted. The terms of this license +apply to the use and Distribution of the Standard or Modified Versions +as included in the aggregation. + +(8) You are permitted to link Modified and Standard Versions with +other works, to embed the Package in a larger work of your own, or to +build stand-alone binary or bytecode versions of applications that +include the Package, and Distribute the result without restriction, +provided the result does not expose a direct interface to the Package. + + +Items That are Not Considered Part of a Modified Version + +(9) Works (including, but not limited to, modules and scripts) that +merely extend or make use of the Package, do not, by themselves, cause +the Package to be a Modified Version. In addition, such works are not +considered parts of the Package itself, and are not subject to the +terms of this license. + + +General Provisions + +(10) Any use, modification, and distribution of the Standard or +Modified Versions is governed by this Artistic License. By using, +modifying or distributing the Package, you accept this license. Do not +use, modify, or distribute the Package, if you do not accept this +license. + +(11) If your Modified Version has been derived from a Modified +Version made by someone other than you, you are nevertheless required +to ensure that your Modified Version complies with the requirements of +this license. + +(12) This license does not grant you the right to use any trademark, +service mark, tradename, or logo of the Copyright Holder. + +(13) This license includes the non-exclusive, worldwide, +free-of-charge patent license to make, have made, use, offer to sell, +sell, import and otherwise transfer the Package with respect to any +patent claims licensable by the Copyright Holder that are necessarily +infringed by the Package. If you institute patent litigation +(including a cross-claim or counterclaim) against any party alleging +that the Package constitutes direct or contributory patent +infringement, then this Artistic License to you shall terminate on the +date that such litigation is filed. + +(14) Disclaimer of Warranty: +THE PACKAGE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS +IS' AND WITHOUT ANY EXPRESS OR IMPLIED WARRANTIES. THE IMPLIED +WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR +NON-INFRINGEMENT ARE DISCLAIMED TO THE EXTENT PERMITTED BY YOUR LOCAL +LAW. UNLESS REQUIRED BY LAW, NO COPYRIGHT HOLDER OR CONTRIBUTOR WILL +BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES ARISING IN ANY WAY OUT OF THE USE OF THE PACKAGE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta.hpp new file mode 100644 index 0000000000..6fd6da797d --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta.hpp @@ -0,0 +1,19 @@ +/* + * meta.hpp + * + * Created on: 26 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef PUSHKIN_META_HPP_ +#define PUSHKIN_META_HPP_ + +#include +#include + +#include +#include +#include +#include + +#endif /* PUSHKIN_META_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/algorithm.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/algorithm.hpp new file mode 100644 index 0000000000..a99fff0a9a --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/algorithm.hpp @@ -0,0 +1,736 @@ +/* + * algorithm.hpp + * + * Created on: 28 мая 2016 г. + * Author: sergey.fedorov + */ + +/** + * @page Metaprogramming algorithms + * + */ +#ifndef PUSHKIN_META_ALGORITHM_HPP_ +#define PUSHKIN_META_ALGORITHM_HPP_ + +#include +#include + +#include +#include + +namespace psst { +namespace meta { + +//@{ +/** + * Metafunction to determine if typename T is contained in type + * variadic pack Y ... + */ +template < typename T, typename ... Y > +struct contains; +template < typename T, typename V, typename ... Y > +struct contains< T, V, Y ... > : contains < T, Y ...> {}; +template < typename T, typename ... Y > +struct contains< T, T, Y ... > : ::std::true_type {}; +template < typename T > +struct contains< T, T > : ::std::true_type {}; +template < typename T, typename Y > +struct contains< T, Y > : ::std::false_type {}; +template < typename T > +struct contains : ::std::false_type {}; +template < typename T, typename ... Y > +struct contains< T, type_tuple > : contains {}; + +template < typename T, typename ... Y > +using contains_t = typename contains::type; +template < typename T, typename ...Y > +constexpr bool contains_v = contains_t::value; +//@} + +//@{ +/** + * Metafunction to determine if a variadic pack is empty + */ +template < typename ... T > +struct is_empty : ::std::false_type {}; +template <> +struct is_empty<> : ::std::true_type {}; +template <> +struct is_empty : ::std::true_type {}; +template < typename ... T > +struct is_empty< type_tuple > + : ::std::conditional< + (sizeof ... (T) > 0), + ::std::false_type, + ::std::true_type + >::type {}; +template +struct is_empty<::std::integer_sequence> : ::std::true_type {}; +template +struct is_empty<::std::integer_sequence> : ::std::false_type {}; + +template < typename ... T > +using is_empty_t = typename is_empty::type; +template < typename ... T > +constexpr bool is_empty_v = is_empty_t::value; +//@} + +//@{ +/** + * Metafunction to get the fist type from a variadic pack of types + * or the first value from an integer_sequence. + */ +template < typename ... T > +struct front; +template < typename ... T> +using front_t = typename front::type; + +template <> +struct front<> { + using type = void; +}; + +template < typename T, typename ... Y > +struct front { + using type = T; +}; + +template < typename ... T > +struct front< type_tuple > : front {}; + +template < typename T, T V, T... Values> +struct front<::std::integer_sequence> : std::integral_constant {}; +template < typename T > +struct front<::std::integer_sequence> { + using type = void; +}; +//@} + +//@{ +/** + * Metafunction to get the last type from a variadic pack of types + * or the last value from an integer_sequnce + */ +template < typename ... T > +struct back; +template < typename ... T > +using back_t = typename back::type; + +template <> +struct back<> { + using type = void; +}; + +template < typename T > +struct back { + using type = T; +}; + +template < typename T, typename ... Y > +struct back : back {}; + +template < typename ... T > +struct back> : back {}; + +template < typename T, T V > +struct back<::std::integer_sequence> : std::integral_constant {}; +template < typename T, T V, T... Values> +struct back<::std::integer_sequence> : back<::std::integer_sequence> {}; +template < typename T > +struct back<::std::integer_sequence> { + using type = void; +}; +//@} + +//@{ +/** + * Metafunction to find an index of T in variadic pack Y... + */ +template < typename T, typename ... Y > +struct index_of; + +namespace detail { + +template < typename T, ::std::size_t N, typename ... Y > +struct index_of_impl; + +template < typename T, ::std::size_t N, typename V, typename ... Y > +struct index_of_impl< T, N, V, Y ... > : index_of_impl {}; +template < typename T, ::std::size_t N, typename ... Y > +struct index_of_impl< T, N, T, Y ... > { + static constexpr ::std::size_t value = N; + static constexpr bool found = true; +}; + +template < typename T, ::std::size_t N > +struct index_of_impl< T, N, T > { + static constexpr ::std::size_t value = N; + static constexpr bool found = true; +}; + +template < typename T, ::std::size_t N, typename Y> +struct index_of_impl { + static constexpr ::std::size_t value = ::std::numeric_limits<::std::size_t>::max(); + static constexpr bool found = false; +}; + +template < typename T, ::std::size_t N> +struct index_of_impl { + static constexpr ::std::size_t value = ::std::numeric_limits<::std::size_t>::max(); + static constexpr bool found = false; +}; + +} /* namespace detail */ + +template < typename T, typename ... Y > +struct index_of : detail::index_of_impl {}; +template < typename T, typename ... Y > +struct index_of< T, type_tuple > : detail::index_of_impl {}; + +template < typename T, typename ... Y > +using index_of_t = typename index_of::type; +//@} + +//@{ +/** + * Metafunction to combine two or more type tuples + */ +template < typename ... T > +struct combine; + +template < typename ... T > +using combine_t = typename combine::type; + +template <> +struct combine<> { + using type = type_tuple<>; +}; + +template < typename T > +struct combine < T > { + using type = type_tuple; +}; + +template < typename ... T > +struct combine < type_tuple > { + using type = type_tuple; +}; + +template < typename ... T, typename U, typename ... Y > +struct combine< type_tuple, U, Y...> + : combine< type_tuple, Y...> {}; + +template < typename T, typename ... U, typename ... Y > +struct combine< T, type_tuple, Y... > : + combine, Y...>{}; + + +template < typename ... T, typename ... U, typename ... Y > +struct combine< type_tuple, type_tuple, Y... > { + using type = combine_t< type_tuple, Y...>; +}; +//@} + +//@{ +/** + * Metafunction to get a part of variadic type pack + */ +template < ::std::size_t Fist, ::std::size_t Last, typename ... T > +struct slice; + +namespace detail { + +template +struct slice_impl; + +template <::std::size_t ... Indexes, typename ... T> +struct slice_impl< ::std::index_sequence, type_tuple > { + using index_tuple = ::std::index_sequence; + static_assert(front_t::value < sizeof...(T), "Start index of slice is out of range"); + static_assert(back_t::value < sizeof...(T), "End index of slice is out of range"); + using types = type_tuple; + using type = type_tuple< typename types::template type... >; +}; + +template +struct slice_impl< ::std::index_sequence<>, type_tuple > { + using type = type_tuple<>; +}; + +} // namespace detail + +template < ::std::size_t Fist, ::std::size_t Last, typename ... T > +struct slice : detail::slice_impl< make_index_sequence, type_tuple > {}; +template < ::std::size_t Fist, ::std::size_t Last, typename ... T > +using slice_t = typename slice::type; + +template < ::std::size_t Fist, ::std::size_t Last, typename ... T > +struct slice> : slice {}; +//@} + +//@{ +/** + * Metafunction to reverse a variadic type pack or a type tuple + */ +template < typename ... T > +struct reverse { + using type = slice_t< sizeof ... (T) - 1, 0, T...>; +}; +template < typename ... T > +using reverse_t = typename reverse::type; + +template <> +struct reverse<> { + using type = type_tuple<>; +}; + +template < typename ... T > +struct reverse> : reverse {}; +//@} + +//@{ +/** + * Metafunction to push a type to the back of type tuple + */ +template < typename T, typename Y > +struct push_back; +template < typename T, typename Y > +using push_back_t = typename push_back::type; + +template < typename ... T, typename Y > +struct push_back, Y > { + using type = type_tuple; +}; +//@} + +//@{ +/** + * Metafunction to push a type to the from of type tuple + */ +template < typename T, typename Y > +struct push_front; +template < typename T, typename Y > +using push_front_t = typename push_front::type; + +template < typename ... T, typename Y > +struct push_front, Y > { + using type = type_tuple; +}; +//@} + +//@{ +/** + * Metafunction to remove a type from the front of a variadic type pack + * or a type tuple. + */ +template < typename ... T > +struct pop_front; +template < typename ... T > +using pop_front_t = typename pop_front::type; + +template < typename T, typename ... Y > +struct pop_front { + using type = type_tuple; +}; + +template <> +struct pop_front<> { + using type = type_tuple<>; +}; + +template < typename ... T > +struct pop_front< type_tuple > : pop_front {}; +//@} + +//@{ +/** + * Metafunction to remove a type from the back of a variadic type pack + * or a type tuple. + */ +template < typename ... T > +struct pop_back; +template < typename ... T > +using pop_back_t = typename pop_back::type; + +template +struct pop_back { + using type = slice_t<0, sizeof...(T) - 2, T...>; +}; + +template < typename T, typename Y > +struct pop_back { + using type = type_tuple; +}; +template < typename T > +struct pop_back { + using type = type_tuple<>; +}; +template <> +struct pop_back<> { + using type = type_tuple<>; +}; + +template +struct pop_back> : pop_back {}; +//@} + +//@{ +/** + * Metafunction to remove a type from a variadic type pack or type tuple + */ +template < typename T, typename ... Y> +struct remove_type; +template < typename T, typename ... Y> +using remove_type_t = typename remove_type::type; + +template < typename T, typename V, typename ... Y > +struct remove_type { + using type = push_front_t, V >; +}; + +template < typename T, typename ... Y > +struct remove_type { + using type = remove_type_t; +}; + +template < typename T > +struct remove_type { + using type = type_tuple<>; +}; + +template < typename T, typename ... Y > +struct remove_type< type_tuple, T > { + using type = remove_type_t; +}; +//@} + +//@{ +/** + * Metafunciton to do nothing with a variadic type pack + */ +template < typename T, typename ... Y > +struct noop { + using type = type_tuple; +}; +//@} + +//@{ +/** + * Metafunction to insert a type to a variadic type pack + * or type tuple only if the pack or tuple doesn't contain it + */ +template < typename T, typename ... Y > +struct insert_type : ::std::conditional_t< + contains_v, + noop, + ::std::conditional_t< + ::std::is_same< T, void >::value, + noop, + push_front, T> + > + > {}; +template < typename T, typename ... Y > +using insert_type_t = typename insert_type::type; + +template < typename T, typename ... Y > +struct insert_type< type_tuple, T > { + using type = insert_type_t; +}; +//@} + +//@{ +/** + * Metafunction to build a type tuple containing only + * unique types from a variadic type pack or type tuple + */ +template < typename ... T > +struct unique; +template < typename ... T> +using unique_t = typename unique::type; + +template < typename T, typename ... Y > +struct unique { + using type = insert_type_t, T>; +}; + +template <> +struct unique<> { + using type = type_tuple<>; +}; + +template < typename ... T > +struct unique< type_tuple > : unique {}; +template < typename ... T, typename ... Y > +struct unique< type_tuple, type_tuple > + : unique {}; + +template < typename ... T, typename ... Y > +struct unique< unique, unique > + : unique {}; +//@} + +template < typename T, typename ... Y > +struct contains< T, unique > : contains {}; + +//@{ +/** + * Check if all types in a variadic type pack or type tuple + * match a predicate. + */ +template < template class Predicate, typename ... T > +struct all_match; + +template < template class Predicate, typename T, typename ... Y > +struct all_match< Predicate, T, Y... > + : ::std::conditional< + Predicate::value, + all_match, + ::std::false_type + >::type {}; + +template < template class Predicate, typename T > +struct all_match< Predicate, T > + : ::std::conditional< + Predicate::value, + ::std::true_type, + ::std::false_type + >::type {}; + +template < template class Predicate > +struct all_match< Predicate > + : ::std::false_type {}; + +template < template class Predicate, typename ... T > +struct all_match< Predicate, type_tuple > + : all_match {}; +//@} + +//@{ +/** + * Check if any type in a variadic type pack or type tuple + * matches a predicate. + */ +template < template class Predicate, typename ... T> +struct any_match; + +template < template class Predicate, typename T, typename ... Y > +struct any_match< Predicate, T, Y... > + : ::std::conditional< + Predicate::value, + ::std::true_type, + any_match + >::type {}; + +template < template class Predicate, typename T > +struct any_match< Predicate, T > + : std::conditional< + Predicate::value, + ::std::true_type, + ::std::false_type + >::type {}; + +template < template class Predicate > +struct any_match< Predicate > : ::std::false_type {}; + +template < template class Predicate, typename ... T > +struct any_match< Predicate, type_tuple > + : any_match< Predicate, T... >{}; +//@} + +//@{ +/** + * Metafunction to find types satisfying a predicate + * in a variadic type pack or type tuple. + */ +template < template class Predicate, typename ... T > +struct find_if; +template < template class Predicate, typename ... T > +using find_if_t = typename find_if::type; + +template < template class Predicate > +struct find_if { + using type = type_tuple<>; +}; + +template < template class Predicate, typename T, typename ... Y> +struct find_if< Predicate, T, Y... > + : ::std::conditional_t< + Predicate::value, + combine< T, find_if_t>, + find_if + > {}; + +template < template class Predicate, typename T > +struct find_if< Predicate, T > + : ::std::conditional_t< + Predicate::value, + combine, + combine<> + > {}; + +template < template class Predicate, typename ... T> +struct find_if< Predicate, type_tuple > : find_if< Predicate, T... > {}; +//@} + +//@{ +/** + * Metafunction to remove types matching a predicate. + */ +template < template class Predicate, typename ... T > +struct remove_if; +template < template class Predicate, typename ... T > +using remove_if_t = typename remove_if::type; + +template < template class Predicate > +struct remove_if< Predicate > { + using type = type_tuple<>; +}; + +template < template class Predicate, typename T, typename ... Y > +struct remove_if + : ::std::conditional_t< + Predicate::value, + remove_if, + combine< T, remove_if_t > + > {}; + +template < template class Predicate, typename T > +struct remove_if + : ::std::conditional_t< + Predicate::value, + combine<>, + combine + > {}; + +template < template class Predicate, typename ... T> +struct remove_if< Predicate, type_tuple > : remove_if< Predicate, T... > {}; +//@} + +//@{ +/** + * Metafunction to apply a predicate to a variadic type pack or + * type tuple. + */ +template < template class Predicate, typename ... T > +struct transform { + using type = type_tuple< typename Predicate::type ... >; +}; + +template < template class Predicate, typename ... T > +struct transform < Predicate, type_tuple > + : transform< Predicate, T... > {}; + +template < template class Predicate > +struct transform { + using type = type_tuple<>; +}; +//@} + +//@{ +/** + * Metafunction to invert a predicate. + * + * Can be used in metafunction 'binding' as follows: + * @code + * template + * using not_fundamental = invert; + * @endcode + */ +template < template class Predicate, typename T > +struct invert : ::std::integral_constant::value> {}; +//@} + +//@{ +/** + * Metafunction to compare two types using a Compare predicate. + * If the Compare predicate is false for both ways of comparison, + * meaning the types are evaluated equal, the order of types is not changed. + */ +template class Compare, typename T, typename Y> +struct stable_order { + using forward = type_tuple; + using reverse = type_tuple; + static constexpr bool original_order = Compare::value || !Compare::value; + using type = ::std::conditional_t; + using first = typename type::template type<0>; + using second = typename type::template type<1>; +}; +//@} + +//@{ +/** + * Metafunction to merge sorted type tuples in a stable sorted order + */ +template class Compare, typename ... T> +struct merge_sorted; +template class Compare, typename ... T> +using merge_sorted_t = typename merge_sorted::type; + +template class Compare> +struct merge_sorted { + using type = type_tuple<>; +}; + +template class Compare, typename ... T> +struct merge_sorted> { + using type = type_tuple; +}; + +template class Compare, typename ... T> +struct merge_sorted, type_tuple<>> { + using type = type_tuple; +}; + +template class Compare, typename ... T> +struct merge_sorted, type_tuple> { + using type = type_tuple; +}; + +template class Compare, typename T1, typename ...TN, typename Y1, typename ... YN> +struct merge_sorted, type_tuple> + : ::std::conditional_t< + stable_order::original_order, + combine< type_tuple, merge_sorted_t, type_tuple> >, + combine< type_tuple, merge_sorted_t, type_tuple> > + > {}; +//@} + +//@{ +/** + * Metafunction to stable sort a list of types using the Compare predicate. + */ +template < template class Compare, typename ... T > +struct stable_sort; +template < template class Compare, typename ... T > +using stable_sort_t = typename stable_sort::type; + +template < template class Compare, typename ... T > +struct stable_sort { + static constexpr std::size_t tuple_size = sizeof...(T); + using first_half = slice_t<0, tuple_size/2, T...>; + using second_half = slice_t; + using type = merge_sorted_t, stable_sort_t>; +}; + +template < template class Compare, typename ... T > +struct stable_sort> : stable_sort {}; + +template < template class Compare, typename T, typename Y > +struct stable_sort : stable_order {}; + +template < template class Compare, typename T > +struct stable_sort { + using type = type_tuple; +}; + +template < template class Compare > +struct stable_sort { + using type = type_tuple<>; +}; +//@} + +} /* namespace meta */ +} /* namespace pus */ + +#endif /* PUSHKIN_META_ALGORITHM_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/callable.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/callable.hpp new file mode 100644 index 0000000000..6036f3d74c --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/callable.hpp @@ -0,0 +1,118 @@ +/* + * callable.hpp + * + * Created on: 29 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef PUSHKIN_META_CALLABLE_HPP_ +#define PUSHKIN_META_CALLABLE_HPP_ + +#include +#include +#include + +namespace psst { +namespace meta { + +template +struct is_callable { +private: + using args_tuple = ::std::tuple; + using indexes = typename index_builder< sizeof ... (Args) >::type; + static args_tuple& args; + + template + static ::std::true_type + test(indexes_tuple const&, + decltype( ::std::declval()( ::std::forward(::std::get(args))... ) ) const*); + template + static ::std::false_type + test(...); +public: + static constexpr bool value = decltype( test(indexes{}, nullptr) )::value; +}; + +template < typename Predicate > +struct not_ { + template < typename ... Args > + bool + operator()(Args&& ... args) const + { + return !Predicate{}(::std::forward(args)...); + } +}; + +template < typename ... Predicates > +struct and_; +template < typename ... Predicates > +struct or_; + +template < typename Predicate, typename ... Rest > +struct and_ { + template < typename ... Args > + bool + operator()(Args&& ... args) const + { + return Predicate{}(::std::forward(args)...) + && and_{}(::std::forward(args)...); + } +}; + +template < typename Predicate > +struct and_ { + template < typename ... Args > + bool + operator()(Args&& ... args) const + { + return Predicate{}(::std::forward(args)...); + } +}; + +template <> +struct and_<> { + template < typename ... Args > + bool + operator()(Args&& ...) const + { + return false; + } +}; + +template < typename Predicate, typename ... Rest > +struct or_ { + template < typename ... Args > + bool + operator()(Args&& ... args) const + { + return Predicate{}(::std::forward(args)...) + || and_{}(::std::forward(args)...); + } +}; + +template < typename Predicate > +struct or_ { + template < typename ... Args > + bool + operator()(Args&& ... args) const + { + return Predicate{}(::std::forward(args)...); + } +}; + +template <> +struct or_<> { + template < typename ... Args > + bool + operator()(Args&& ...) const + { + return true; + } +}; + +} /* namespace meta */ +} /* namespace pus */ + + + +#endif /* PUSHKIN_META_CALLABLE_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/char_sequence.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/char_sequence.hpp new file mode 100644 index 0000000000..80fba45b8a --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/char_sequence.hpp @@ -0,0 +1,509 @@ +/* + * char_sequence.hpp + * + * Created on: Mar 9, 2017 + * Author: zmij + */ + +#ifndef PUSHKIN_META_CHAR_SEQUENCE_HPP_ +#define PUSHKIN_META_CHAR_SEQUENCE_HPP_ + +#include + +namespace psst { +namespace meta { + +template < char C > +using char_constant = ::std::integral_constant; + +namespace detail { + +template < char C, char ... Chars > +struct first_char { + using type = ::std::integral_constant; +}; + +template < char C, char ... Chars > +struct last_char : last_char< Chars ... > {}; +template < char C > +struct last_char { + using type = ::std::integral_constant; +}; + +constexpr ::std::size_t +strlen(char const* str) +{ + ::std::size_t sz = 0; + for (; *str != 0; ++sz, ++str); + return sz; +} + +constexpr int +charcmp(char lhs, char rhs) +{ + return (lhs < rhs) ? - 1 : (lhs > rhs ? 1 : 0); +} + +constexpr int +strcmp(char const* lhs, char const* rhs) +{ + int cmp = 0; + for (; cmp == 0 && *lhs != 0 && *rhs != 0; ++lhs, ++rhs) { + cmp = charcmp(*lhs, *rhs); + } + if (cmp == 0 && (*lhs != 0 || *rhs != 0)) { + return *lhs != 0 ? 1 : -1; + } + return cmp; +} + +constexpr char +min(char a, char b) +{ return a < b ? a : b; } + +constexpr char +max(char a, char b) +{ return a > b ? a : b; } + +template < char Min, char Max, char ... Chars > +struct unwrap_char_range; + +} /* namespace detail */ + +//---------------------------------------------------------------------------- +template < char ... Chars > +struct char_sequence_literal { + using type = char_sequence_literal; + static constexpr char value[]{ Chars..., 0 }; + static constexpr ::std::size_t size = sizeof ... (Chars); + + static constexpr bool + eq(char const* str) + { + return detail::strcmp(value, str) == 0; + } + static constexpr bool + ne(char const* str) + { + return detail::strcmp(value, str) != 0; + } + static constexpr bool + lt(char const* str) + { + return detail::strcmp(value, str) < 0; + } + static constexpr bool + gt(char const* str) + { + return detail::strcmp(value, str) > 0; + } + static constexpr bool + le(char const* str) + { + return detail::strcmp(value, str) <= 0; + } + static constexpr bool + ge(char const* str) + { + return detail::strcmp(value, str) >= 0; + } + + /** + * Check if a string starts with this char literal + * @param str + * @return + */ + template < typename InputIterator > + static constexpr bool + starts_with(InputIterator str) + { + if (size == 0) + return true; + char const* lhs = value; + for (; *lhs != 0 && *str != 0; ++lhs, ++str) { + if (*lhs != *str) + return false; + } + + return *lhs == 0; + } + + static constexpr char const* + static_begin() + { return value; } + static constexpr char const* + static_end() + { return value + size; } +}; + +template < char ... Chars > +constexpr char char_sequence_literal::value[]; + +template < typename T > +struct make_char_literal; + +//---------------------------------------------------------------------------- +/** + * A template structure representing a character sequence + */ +template < char ... Chars > +struct char_sequence { + using size = ::std::integral_constant<::std::size_t, sizeof ... (Chars)>; + using first_char = typename detail::first_char::type; + using last_char = typename detail::last_char::type; +}; + +template <> +struct char_sequence<> { + using size = ::std::integral_constant<::std::size_t, 0>; +}; + + +template < char ... Chars > +struct make_char_literal> { + using type = char_sequence_literal; +}; + +//---------------------------------------------------------------------------- +/** + * A range of chars + */ +template < char Min, char Max > +struct char_range { + static_assert(Min <= Max, "Minimum char must be less or equal to maximum"); + using size = ::std::integral_constant<::std::size_t, Max - Min + 1>; + using first_char = ::std::integral_constant; + using last_char = ::std::integral_constant; +}; + +namespace chars { +//---------------------------------------------------------------------------- +//@{ +template < typename T, typename U > +struct includes; + +template < char MinA, char MaxA, char MinB, char MaxB > +struct includes< char_range< MinA, MaxA >, char_range< MinB, MaxB > > { + using type = ::std::integral_constant< bool, ( MinA <= MinB && MaxB <= MaxA ) >; +}; +//@} + +//---------------------------------------------------------------------------- +//@{ +template < typename T, typename U > +struct overlaps; + +template < char MinA, char MaxA, char MinB, char MaxB > +struct overlaps , char_range< MinB, MaxB > > { + using type = ::std::integral_constant< bool, !( MaxA < MinB || MaxB < MinA ) >; +}; +//@} + +//---------------------------------------------------------------------------- +//@{ +template < typename T, char> +struct contains; + +template < char F, char ... O, char C > +struct contains< char_sequence, C > : contains< char_sequence, C > {}; + +template < char ... O, char C > +struct contains< char_sequence, C > : ::std::true_type {}; + +template < char C > +struct contains< char_sequence<>, C > : ::std::false_type {}; + +template < char Min, char Max, char C > +struct contains< char_range, C > : + ::std::integral_constant {}; +//@} + +} /* namespace chars */ + +namespace detail { + +template < char Min, char Max, char ... Chars > +struct unwrap_char_range { + static_assert(Min < Max, "Starting char of the range must be less than ending"); + using type = typename unwrap_char_range< Min, Max - 1, Max, Chars... >::type; +}; + +template < char C, char ... Chars > +struct unwrap_char_range< C, C, Chars ... > { + using type = char_sequence; +}; + +#if __cplusplus >= 201402L +/** + * Convert a pointer to char to a char_sequence, compile time. + */ +template < char const* Str, typename T > +struct make_char_sequence_impl; + +template < char const* Str, ::std::size_t ... Indexes > +struct make_char_sequence_impl> { + using type = char_sequence< Str[Indexes]... >; +}; + +/** + * Build a table for sorting chars + */ +template < typename IndexTuple, template < char > class Predicate, + typename V, V ifTrue, V ifFalse = V{} > +struct build_sort_table; + +template < ::std::size_t ... Indexes, template < char > class Predicate, + typename V, V ifTrue, V ifFalse > +struct build_sort_table<::std::integer_sequence< ::std::size_t, Indexes... >, + Predicate, V, ifTrue, ifFalse > { + + using size = ::std::integral_constant<::std::size_t, sizeof ... (Indexes)>; + using type = ::std::integer_sequence< V, + (Predicate< (char)Indexes >::value ? ifTrue : ifFalse) ... >; +}; + +template < typename T, typename Index > +struct make_char_sort_table; + +template < char ... Chars, typename Index > +struct make_char_sort_table, Index> { + using charset = char_sequence; + template < char C > + using predicate = chars::contains; + using type = typename build_sort_table::type; +}; + +template < unsigned char N, typename T, char ... Chars > +struct sort_helper; + +template < unsigned char N, bool V, bool ... Vals, char ... Chars > +struct sort_helper, Chars... > { + using type = typename ::std::conditional, char(N), Chars...>, + sort_helper, Chars...> + >::type::type; +}; + +template < unsigned char N, bool V, char ... Chars > +struct sort_helper< N, ::std::integer_sequence, Chars ... > { + using type = typename ::std::conditional, + char_sequence< Chars... > + >::type; +}; + +#endif /* __cplusplus >= 201402L */ + +} /* namespace detail */ + +#if __cplusplus >= 201402L +template < typename T > +struct unique_sort; + +/** + * Uniquely sort a sequence of chars + */ +template < char ... Chars > +struct unique_sort> { + using original_seq = char_sequence; + using low_index = typename detail::reverse_unwrap_range<::std::size_t, 0, 127>::type; + using high_index = typename detail::reverse_unwrap_range<::std::size_t, 128, 255>::type; + using low_table = typename detail::make_char_sort_table::type; + using high_table = typename detail::make_char_sort_table::type; + using low_sorted = typename detail::sort_helper< 127, low_table >::type; + using high_sorted = typename detail::sort_helper< 127, high_table >::type; + + using type = typename join< low_sorted, high_sorted >::type; +}; +#endif /* __cplusplus >= 201402L */ + +/** + * Specialization to join two character sequences + */ +template < char ... A, char ... B > +struct join< char_sequence, char_sequence > { + using type = char_sequence; +}; + +template < char ... A, char C > +struct join< char_sequence, ::std::integral_constant< char, C > > { + using type = char_sequence< A..., C >; +}; + +template < char ... A, char C > +struct join< ::std::integral_constant< char, C >, char_sequence > { + using type = char_sequence< A..., C >; +}; + +//@{ +/** @name Specializations to join adjacent character ranges */ +template < char A, char B, char C > +struct join< char_range, char_range > { + using type = char_range< A, C >; +}; + +template < char A, char B, char C > +struct join< char_range, char_range > { + using type = char_range< A, C >; +}; + +template < char A, char B, char C > +struct join< char_range, char_range > { + using type = char_range< A, C >; +}; + +template < char A, char B, char C > +struct join< char_range, char_range > { + using type = char_range< A, C >; +}; + +template < char A, char B > +struct join< char_range, ::std::integral_constant > { + using type = char_range; +}; +template < char A, char B > +struct join< char_range, ::std::integral_constant > { + using type = char_range; +}; + +template < char A, char B > +struct join< char_range, ::std::integral_constant > { + using type = char_range; +}; +template < char A, char B > +struct join< char_range, ::std::integral_constant > { + using type = char_range; +}; + +//@} + +#if __cplusplus >= 201402L +namespace detail { + +template < char A, char B, char C, char D, bool > +struct overlapping_range_join { + using type = char_range< min(A, C), max(B, D) >; +}; + +template < char A, char B, char C, char D > +struct overlapping_range_join< A, B, C, D, false > { + using type = typename unique_sort< + typename join< + typename unwrap_char_range< A, B >::type, + typename unwrap_char_range< C, D >::type + >::type + >::type; +}; + +template < char A, char B, char C, bool > +struct including_range_join { + using type = char_range< A, B >; +}; + +template < char A, char B, char C > +struct including_range_join { + using type = typename unique_sort< + typename join< + typename unwrap_char_range::type, + ::std::integral_constant + >::type + >::type; +}; + +} /* namespace detail */ + +template < char A, char B, char C, char D > +struct join< char_range, char_range > + : detail::overlapping_range_join< A, B, C, D, + chars::overlaps< char_range, + char_range>::type::value > {}; + +template < char A, char B, char C > +struct join< char_range, ::std::integral_constant > + : detail::including_range_join< + A, B, C, + chars::contains, C>::value> {}; + +template < char A, char B, char C > +struct join< ::std::integral_constant, char_range > + : detail::including_range_join< + A, B, C, + chars::contains, C>::value> {}; + +//---------------------------------------------------------------------------- +template < char A, char B > +struct join< char_range, char_sequence<> > { + using type = char_range; +}; + +template < char A, char B, char ... Chars > +struct join< char_range, char_sequence > { + using type = typename unique_sort< + typename join< + typename detail::unwrap_char_range< A, B >::type, + char_sequence< Chars... > + >::type + >::type; +}; + +template < char A, char B > +struct join< char_sequence<>, char_range > { + using type = char_range; +}; + +template < char ... Chars, char A, char B > +struct join< char_sequence, char_range > { + using type = typename unique_sort< + typename join< + typename detail::unwrap_char_range< A, B >::type, + char_sequence< Chars... > + >::type + >::type; +}; + +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +template < char const* Str > +using make_char_sequence = typename detail::make_char_sequence_impl>::type; + +template < char A, char B > +struct make_char_literal> { + using type = typename make_char_literal< + typename detail::unwrap_char_range< A, B >::type + >::type; +}; + +//template < char const* Str > +namespace detail { + +/** + * Make a char sequence literal from a pointer to char, compile-time + */ +template < char const* Str, typename T > +struct make_char_literal_impl; + +template < char const* Str, ::std::size_t ... Indexes > +struct make_char_literal_impl> { + using type = char_sequence_literal< Str[Indexes]... >; +}; + +} /* namespace detail */ + + +template < char const* Str > +using make_char_literal_s = typename detail::make_char_literal_impl>::type; + +#endif /* __cplusplus >= 201402L */ + +} /* namespace meta */ +} /* namespace psst */ + +#ifdef __METASHELL +#include +#endif + +#endif /* PUSHKIN_META_CHAR_SEQUENCE_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/function_traits.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/function_traits.hpp new file mode 100644 index 0000000000..1245f36630 --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/function_traits.hpp @@ -0,0 +1,256 @@ +/* + * function_traits.hpp + * + * Created on: Feb 2, 2016 + * Author: zmij + */ + +#ifndef PSST_META_FUNCTION_TRAITS_HPP_ +#define PSST_META_FUNCTION_TRAITS_HPP_ + +#include +#include + +namespace psst { +namespace meta { + +namespace detail { + +template +struct has_call_operator { +private: + struct _fallback { void operator()(); }; + struct _derived : T, _fallback {}; + + template struct _check; + + template + static ::std::true_type test(...); + + template + static ::std::false_type test( + _check*); + +public: + static const bool value = + ::std::is_same< decltype(test<_derived>(0)), ::std::true_type >::value; +}; + +} // namespace detail + +template < typename T > +struct is_callable_object : ::std::conditional< + ::std::is_class< T >::value, + detail::has_call_operator< T >, + ::std::is_function >::type { +}; + +/** + * Primary function_traits template + */ +template < typename ... T > +struct function_traits; + +template < typename T > +struct not_a_function { + static const int arity = -1; +}; + +/** + * function_traits for a function pointer with argument count > 1 + */ +template < typename Return, typename ... Args > +struct function_traits< Return(*)(Args...) > { + enum { arity = sizeof...(Args) }; + + using result_type = Return; + using args_tuple_type = ::std::tuple< Args ... >; + using decayed_args_tuple_type = ::std::tuple< typename ::std::decay::type ... >; + template < size_t n> + struct arg { + using type = typename ::std::tuple_element::type; + }; +}; + +/** + * function_traits for a function pointer with argument count == 1 + */ +template < typename Return, typename Arg > +struct function_traits< Return(*)(Arg) > { + enum { arity = 1 }; + + using result_type = Return; + using args_tuple_type = Arg; + using decayed_args_tuple_type = typename ::std::decay::type; +}; + +/** + * function_traits for a function pointer with argument count == 0 + */ +template < typename Return > +struct function_traits< Return(*)() > { + enum { arity = 0 }; + using result_type = Return; + using args_tuple_type = void; + using decayed_args_tuple_type = void; +}; + +/** + * function_traits for a class member const function with argument count > 1 + */ +template < typename Class, typename Return, typename ... Args > +struct function_traits< Return(Class::*)(Args...) const> { + enum { arity = sizeof...(Args) }; + + using class_type = Class; + using result_type = Return; + using args_tuple_type = ::std::tuple< Args ... >; + using decayed_args_tuple_type = ::std::tuple< typename ::std::decay::type ... >; + + template < size_t n> + struct arg { + using type = typename ::std::tuple_element::type; + }; +}; + +/** + * function_traits for a class member const function with argument count == 1 + */ +template < typename Class, typename Return, typename Arg > +struct function_traits< Return(Class::*)(Arg) const > { + enum { arity = 1 }; + + using class_type = Class; + using result_type = Return; + using args_tuple_type = Arg; + using decayed_args_tuple_type = typename ::std::decay::type; +}; + +/** + * function_traits for a class member const function with argument count == 0 + */ +template < typename Class, typename Return > +struct function_traits< Return(Class::*)() const> { + enum { arity = 0 }; + + using class_type = Class; + using result_type = Return; + using args_tuple_type = void; + using decayed_args_tuple_type = void; +}; + +/** + * function_traits for a class member non-const function with argument count > 1 + */ +template < typename Class, typename Return, typename ... Args > +struct function_traits< Return(Class::*)(Args...) > { + enum { arity = sizeof...(Args) }; + + using class_type = Class; + using result_type = Return; + using args_tuple_type = ::std::tuple< Args ... >; + using decayed_args_tuple_type = ::std::tuple< typename ::std::decay::type ... >; + template < size_t n> + struct arg { + using type = typename ::std::tuple_element::type; + }; +}; + +/** + * function_traits for a class member non-const function with argument count == 1 + */ +template < typename Class, typename Return, typename Arg > +struct function_traits< Return(Class::*)(Arg) > { + enum { arity = 1 }; + + using class_type = Class; + using result_type = Return; + using args_tuple_type = Arg; + using decayed_args_tuple_type = typename ::std::decay::type; +}; + +/** + * function_traits for a class member non-const function with argument count == 0 + */ +template < typename Class, typename Return > +struct function_traits< Return(Class::*)() > { + enum { arity = 0 }; + + using class_type = Class; + using result_type = Return; + using args_tuple_type = void; + using decayed_args_tuple_type = void; +}; + +template < typename T > +struct call_operator_traits : function_traits< decltype(&T::operator()) > {}; + +template < typename T > +struct function_traits : + ::std::conditional< + is_callable_object< T >::value, + call_operator_traits< T >, + not_a_function >::type {}; + +template < typename Func > +struct is_func_void : + ::std::conditional< + ::std::is_same< typename function_traits< Func >::result_type, void >::value, + ::std::true_type, + ::std::false_type + >::type {}; + +namespace detail { + +template < typename Func, size_t ... Indexes, typename ... T > +typename function_traits::result_type +invoke(Func func, indexes_tuple< Indexes ... >, ::std::tuple< T ... >& args) +{ + return func(::std::get(args) ...); +} + +template < typename Func, size_t ... Indexes, typename ... T > +typename function_traits::result_type +invoke(Func func, indexes_tuple< Indexes ... >, ::std::tuple< T ... >&& args) +{ + return func(::std::move(::std::get(args)) ...); +} + + +template < typename Func, size_t ... Indexes, typename ... T > +typename function_traits::result_type +invoke(Func func, indexes_tuple< Indexes ... >, T&& ... args) +{ + return func(::std::forward(args) ... ); +} + +} // namespace detail + +template < typename Func, typename ... T > +typename function_traits::result_type +invoke(Func func, ::std::tuple< T ... >& args) +{ + using index_type = typename index_builder< sizeof ... (T) >::type; + return detail::invoke(func, index_type(), args); +} + +template < typename Func, typename ... T > +typename function_traits::result_type +invoke(Func func, ::std::tuple< T ... >&& args) +{ + using index_type = typename index_builder< sizeof ... (T) >::type; + return detail::invoke(func, index_type(), ::std::move(args)); +} + +template < typename Func, typename ... T > +typename function_traits::result_type +invoke(Func func, T&& ... args) +{ + using index_type = typename index_builder< sizeof ... (T)>::type; + return detail::invoke(func, index_type{}, ::std::forward(args) ...); +} + +} // namespace meta +} // namespace psst + +#endif /* PSST_META_FUNCTION_TRAITS_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/functions.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/functions.hpp new file mode 100644 index 0000000000..6eaa3e3a7a --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/functions.hpp @@ -0,0 +1,39 @@ +/* + * functions.hpp + * + * Created on: Aug 30, 2016 + * Author: zmij + */ + +#ifndef PUSHKIN_META_FUNCTIONS_HPP_ +#define PUSHKIN_META_FUNCTIONS_HPP_ + +namespace psst { +namespace meta { + +inline bool +any_of(::std::initializer_list args) +{ + for (auto v : args) { + if (v) + return true; + } + return false; +} + +inline bool +all_of(::std::initializer_list args) +{ + for (auto v : args) { + if (!v) + return false; + } + return true; +} + +} /* namespace meta */ +} /* namespace psst */ + + + +#endif /* PUSHKIN_META_FUNCTIONS_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/index_tuple.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/index_tuple.hpp new file mode 100644 index 0000000000..c2f058829c --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/index_tuple.hpp @@ -0,0 +1,38 @@ +/* + * index_tuple.hpp + * + * Created on: 29 мая 2016 г. + * Author: sergey.fedorov + */ + +#ifndef PUSHKIN_META_INDEX_TUPLE_HPP_ +#define PUSHKIN_META_INDEX_TUPLE_HPP_ + +#include + +namespace psst { +namespace meta { + +template < ::std::size_t ... Indexes > +struct indexes_tuple { + static constexpr ::std::size_t size = sizeof ... (Indexes); +}; + +template < ::std::size_t num, typename tp = indexes_tuple <> > +struct index_builder; + +template < ::std::size_t num, ::std::size_t ... Indexes > +struct index_builder< num, indexes_tuple< Indexes ... > > + : index_builder< num - 1, indexes_tuple< Indexes ..., sizeof ... (Indexes) > > { +}; + +template <::std::size_t ... Indexes > +struct index_builder< 0, indexes_tuple< Indexes ... > > { + typedef indexes_tuple < Indexes ... > type; + static constexpr ::std::size_t size = sizeof ... (Indexes); +}; + +} /* namespace meta */ +} /* namespace pus */ + +#endif /* PUSHKIN_META_INDEX_TUPLE_HPP_ */ diff --git a/src/systems/elevator/vender/metapushkin/include/pushkin/meta/integer_sequence.hpp b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/integer_sequence.hpp new file mode 100644 index 0000000000..d6803bc2ca --- /dev/null +++ b/src/systems/elevator/vender/metapushkin/include/pushkin/meta/integer_sequence.hpp @@ -0,0 +1,175 @@ +/* + * integer_sequence.hpp + * + * Created on: Mar 9, 2017 + * Author: zmij + */ + +#ifndef PUSHKIN_META_INTEGER_SEQUENCE_HPP_ +#define PUSHKIN_META_INTEGER_SEQUENCE_HPP_ + +#include +#include + +//#include + +namespace psst { +namespace meta { + +/** + * Metafunction to join sequences + */ +template < typename ... T > +struct join; +template < typename ... T > +using join_t = typename join::type; + +template < typename T > +struct join < T > { + using type = T; +}; + +template < typename T, typename U, typename ... V > +struct join< T, U, V... > + : join< join_t< T, U >, V... > {}; + +template < typename T, T ... A, T... B > +struct join< ::std::integer_sequence, ::std::integer_sequence > { + using type = ::std::integer_sequence; +}; + +template < typename T, T ... A, T B > +struct join< ::std::integer_sequence, ::std::integral_constant > { + using type = ::std::integer_sequence; +}; + +template < typename T, T A, T ...B > +struct join< ::std::integral_constant, ::std::integer_sequence > { + using type = ::std::integer_sequence; +}; + +namespace detail { + +template < typename T, T Min, T Max, T ... Values > +struct unwrap_range_impl { + static_assert(Min < Max, "Start of range must be less than ending"); + using type = typename unwrap_range_impl::type; +}; + +template < typename T, T V, T ... Values > +struct unwrap_range_impl { + using type = ::std::integer_sequence< T, V, Values... >; +}; + +template < typename T, T Min, T Max, T ... Values > +struct reverse_unwrap_range_impl { + static_assert(Min < Max, "Start of range must be less than ending"); + using type = typename reverse_unwrap_range_impl::type; +}; + +template < typename T, T V, T ... Values > +struct reverse_unwrap_range_impl { + using type = ::std::integer_sequence< T, V, Values... >; +}; + +const ::std::size_t template_depth = 128; + +template < typename T, T Min, T Max > +struct unwrap_range; +template < typename T, T Min, T Max > +struct reverse_unwrap_range; + +template < typename T, T Min, T Max > +struct split_range_helper { + using type = typename join< + typename unwrap_range_impl::type, + typename unwrap_range::type + >::type; +}; + +template < typename T, T Min, T Max > +struct unwrap_range : + ::std::conditional< (Max - Min < template_depth), + unwrap_range_impl, + split_range_helper + >::type {}; + +template < typename T, T Min, T Max > +struct reverse_split_range_helper { + using type = typename join< + typename reverse_unwrap_range::type, + typename reverse_unwrap_range_impl::type + >::type; +}; +template < typename T, T Min, T Max > +struct reverse_unwrap_range : + ::std::conditional< (Max - Min < template_depth), + reverse_unwrap_range_impl, + reverse_split_range_helper + >::type {}; + +template < typename T, T Min, T Max, bool Reverse > +struct make_integer_sequence_impl : unwrap_range {}; + +template < typename T, T Min, T Max > +struct make_integer_sequence_impl : reverse_unwrap_range {}; + +} /* namespace detail */ + + +/** + * Metafunction to make integer sequences in range [Min, Max] + * If Min > Max, the sequence will be reversed. + */ +template < typename T, T Min, T Max > +using make_integer_sequence = + typename detail::make_integer_sequence_impl Max )>::type; + +template <::std::size_t Min, ::std::size_t Max> +using make_index_sequence = make_integer_sequence<::std::size_t, Min, Max>; + +namespace detail { + +template < ::std::size_t Head, template class Predicate, typename ... T > +struct find_index_if_impl; + +template < ::std::size_t Head, template class Predicate, typename T, typename ...Y > +struct find_index_if_impl : + ::std::conditional< + Predicate::value, + typename join< ::std::integral_constant<::std::size_t, Head>, + typename find_index_if_impl::type >::type, + typename find_index_if_impl::type + > {}; + +template < ::std::size_t Head, template class Predicate, typename T > +struct find_index_if_impl : + ::std::conditional< + Predicate::value, + ::std::index_sequence, + ::std::index_sequence<> + > {}; + +} /* namespace detail */ + +/** + * Metafunction to build index sequence of types matching predicate + */ +template < template class Predicate, typename ... T > +struct find_index_if : detail::find_index_if_impl<0, Predicate, T...> {}; + +template