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/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index e69a33c7e8..f14f3187cf 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -39,11 +39,6 @@ namespace ignition::gazebo { class ShapesPrivate { - /// \brief Ignition communication node. - public: transport::Node node; - - /// \brief Transform control service name - public: std::string service; }; } diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 2c84fc2648..afbc1477f7 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}) @@ -58,6 +58,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 @@ -98,6 +105,10 @@ add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) add_subdirectory(diff_drive) +# elevator system causes compile erros on windows +if (NOT WIN32) +add_subdirectory(elevator) +endif() add_subdirectory(follow_actor) add_subdirectory(hydrodynamics) add_subdirectory(imu) 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