Migrate to afsm

Signed-off-by: Nick Lamprianidis <nlamprian@gmail.com>

src/systems/elevator/Elevator.cc

@@ -222,7 +222,6 @@ void Elevator::Configure(const Entity &_entity,
   // Initialize state machine
   this->dataPtr->stateMachine =
       std::make_unique<ElevatorStateMachine>(this->dataPtr);
-  this->dataPtr->stateMachine->start();
 
   // Subscribe to command topic
   std::string cmdTopicName =
@@ -247,9 +246,7 @@ void Elevator::PostUpdate(const UpdateInfo &_info,
     return;
   this->dataPtr->lastUpdateTime = _info.simTime;
 
-  this->dataPtr->stateMachine->execute_queued_events();
-
-  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  std::lock_guard<std::recursive_mutex> lock(this->dataPtr->mutex);
   this->dataPtr->UpdateState(_info, _ecm);
   this->dataPtr->doorTimer.Update(
       _info, _ecm, this->dataPtr->isDoorwayBlockedStates[this->dataPtr->state]);
@@ -258,7 +255,9 @@ void Elevator::PostUpdate(const UpdateInfo &_info,
 }
 
 //////////////////////////////////////////////////
-ElevatorPrivate::~ElevatorPrivate() { this->stateMachine->stop(); }
+ElevatorPrivate::~ElevatorPrivate()
+{
+}
 
 //////////////////////////////////////////////////
 bool ElevatorPrivate::InitCabin(const std::string &_cabinJointName,
@@ -366,7 +365,7 @@ bool ElevatorPrivate::InitDoors(const std::string &_doorJointPrefix,
 void ElevatorPrivate::StartDoorTimer(
     int32_t /*_floorTarget*/, const std::function<void()> &_timeoutCallback)
 {
-  std::lock_guard<std::mutex> lock(this->mutex);
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
   this->doorTimer.Configure(this->lastUpdateTime, _timeoutCallback);
 }
 
@@ -375,7 +374,7 @@ void ElevatorPrivate::SetDoorMonitor(
     int32_t _floorTarget, double _jointTarget, double _posEps, double _velEps,
     const std::function<void()> &_jointTargetReachedCallback)
 {
-  std::lock_guard<std::mutex> lock(this->mutex);
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
   this->doorJointMonitor.Configure(this->doorJoints[_floorTarget], _jointTarget,
                                    _posEps, _velEps,
                                    _jointTargetReachedCallback);
@@ -386,7 +385,7 @@ void ElevatorPrivate::SetCabinMonitor(
     int32_t /*_floorTarget*/, double _jointTarget, double _posEps,
     double _velEps, const std::function<void()> &_jointTargetReachedCallback)
 {
-  std::lock_guard<std::mutex> lock(this->mutex);
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
   this->cabinJointMonitor.Configure(this->cabinJoint, _jointTarget, _posEps,
                                     _velEps, _jointTargetReachedCallback);
 }
@@ -422,7 +421,7 @@ void ElevatorPrivate::OnLidarMsg(size_t _floorLevel,
 {
   bool isDoorwayBlocked = _msg.ranges(0) < _msg.range_max() - 0.005;
   if (isDoorwayBlocked == this->isDoorwayBlockedStates[_floorLevel]) return;
-  std::lock_guard<std::mutex> lock(this->mutex);
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
   this->isDoorwayBlockedStates[_floorLevel] = isDoorwayBlocked;
 }
 
@@ -436,7 +435,7 @@ void ElevatorPrivate::OnCmdMsg(const msgs::Int32 &_msg)
             << this->cabinTargets.size() << ")" << std::endl;
     return;
   }
-  this->stateMachine->enqueue_event(EnqueueNewTargetEvent(_msg.data()));
+  this->stateMachine->process_event(events::EnqueueNewTarget(_msg.data()));
 }
 
 IGNITION_ADD_PLUGIN(Elevator, System, Elevator::ISystemConfigure,

src/systems/elevator/ElevatorCommonPrivate.hh

@@ -88,7 +88,7 @@ class ElevatorCommonPrivate
   public: int32_t state{0};
 
   /// \brief A mutex to protect access to targets and state
-  public: std::mutex mutex;
+  public: std::recursive_mutex mutex;
 };
 
 }  // namespace systems

src/systems/elevator/ElevatorStateMachine.hh

@@ -25,13 +25,11 @@
 
 #include <memory>
 
-#include <boost/msm/back/state_machine.hpp>
-#include <boost/msm/front/functor_row.hpp>
-#include <boost/msm/front/state_machine_def.hpp>
-
 #include <ignition/gazebo/Entity.hh>
 #include <ignition/transport/Node.hh>
 
+#include "afsm/fsm.hpp"
+
 namespace ignition
 {
 namespace gazebo
@@ -56,13 +54,6 @@ namespace events
   struct CabinAtTarget;
 }  // namespace events
 
-// State forward declarations
-class IdleState;
-class OpenDoorState;
-class WaitState;
-class CloseDoorState;
-class MoveCabinState;
-
 // Action forward declarations
 namespace actions
 {
@@ -83,65 +74,68 @@ namespace guards
 
 /// \brief Elevator state machine frontend. Defines the transition table and
 /// initial state of the state machine.
-class ElevatorStateMachine_
-    : public boost::msm::front::state_machine_def<ElevatorStateMachine_>
+class ElevatorStateMachineDef
+    : public ::afsm::def::state_machine<ElevatorStateMachineDef>
 {
+  // State forward declarations
+  struct IdleState;
+  template <typename E>
+  struct DoorState;
+  struct OpenDoorState;
+  struct CloseDoorState;
+  struct WaitState;
+  struct MoveCabinState;
+
   /// \brief Constructor
-  public: ElevatorStateMachine_(
+  public: ElevatorStateMachineDef(
       const std::shared_ptr<ElevatorCommonPrivate> &_system);
 
   /// \brief Destructor
-  public: ~ElevatorStateMachine_();
+  public: ~ElevatorStateMachineDef();
 
   /// \brief Gives access to the private data of the state machine. Used by the
   /// states
   /// \return Pointer to the private data of the state machine
   public: const std::unique_ptr<ElevatorStateMachinePrivate> &Data() const;
 
   /// \brief Initial state of the state machine
-  public: using initial_state = boost::mpl::vector<IdleState>;
-
-  /// \brief Alias to make transition table cleaner
-  private: template <typename S, typename E, typename T, typename A, typename G>
-  using Row = boost::msm::front::Row<S, E, T, A, G>;
+  public: using initial_state = IdleState;
 
-  /// \brief Alias to make transition table cleaner
-  private: using none = boost::msm::front::none;
+  /// \brief Transition transition table
+  public: using internal_transitions = transition_table <
+            in<events::EnqueueNewTarget, actions::EnqueueNewTarget<true>,
+               guards::IsInState<IdleState> >,
+            in<events::EnqueueNewTarget, actions::EnqueueNewTarget<false>,
+               guards::IsInState<CloseDoorState> >
+   >;
 
   /// \brief Transition table
-  public: struct transition_table
-      : boost::mpl::vector<
+  public: using transitions = transition_table<
             // +--------------------------------------------------------------+
-            Row<IdleState, EnqueueNewTargetEvent, none,
-                EnqueueNewTargetAction<true>, none>,
-            Row<IdleState, NewTargetEvent, OpenDoorState, NewTargetAction,
-                CabinAtTargetGuard<false> >,
-            Row<IdleState, NewTargetEvent, MoveCabinState, NewTargetAction,
-                CabinAtTargetGuard<true> >,
+            tr<IdleState, events::NewTarget, OpenDoorState, actions::NewTarget,
+               guards::CabinAtTarget>,
+            tr<IdleState, events::NewTarget, MoveCabinState, actions::NewTarget,
+               not_<guards::CabinAtTarget> >,
             // +--------------------------------------------------------------+
-            Row<OpenDoorState, DoorOpenEvent, WaitState, none, none>,
+            tr<OpenDoorState, events::DoorOpen, WaitState, none, none>,
             // +--------------------------------------------------------------+
-            Row<WaitState, TimeoutEvent, CloseDoorState, none, none>,
+            tr<WaitState, events::Timeout, CloseDoorState, none, none>,
             // +--------------------------------------------------------------+
-            Row<CloseDoorState, EnqueueNewTargetEvent, none,
-                EnqueueNewTargetAction<false>, none>,
-            Row<CloseDoorState, DoorClosedEvent, IdleState, none,
-                NoTargetGuard<false> >,
-            Row<CloseDoorState, DoorClosedEvent, MoveCabinState, none,
-                NoTargetGuard<true> >,
+            tr<CloseDoorState, events::DoorClosed, IdleState, none,
+               guards::NoQueuedTarget>,
+            tr<CloseDoorState, events::DoorClosed, MoveCabinState, none,
+               not_<guards::NoQueuedTarget> >,
             // +--------------------------------------------------------------+
-            Row<MoveCabinState, CabinAtTargetEvent, OpenDoorState,
-                CabinAtTargetAction, none> >
-  {
-  };
+            tr<MoveCabinState, events::CabinAtTarget, OpenDoorState,
+               actions::CabinAtTarget, none>
+   >;
 
   /// \brief Private data pointer
   private: std::unique_ptr<ElevatorStateMachinePrivate> dataPtr;
 };
 
 /// \brief Elevator state machine backend
-using ElevatorStateMachine =
-    boost::msm::back::state_machine<ElevatorStateMachine_>;
+using ElevatorStateMachine = ::afsm::state_machine<ElevatorStateMachineDef>;
 
 }  // namespace systems
 }  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE

src/systems/elevator/state_machine/ActionsImpl.hh

@@ -63,7 +63,7 @@ struct NewTarget
   void operator()(const Event &, Fsm &_fsm, Source &, Target &)
   {
     const auto &data = _fsm.Data();
-    std::lock_guard<std::mutex> lock(data->system->mutex);
+    std::lock_guard<std::recursive_mutex> lock(data->system->mutex);
     if (data->targets.front() == data->system->state) data->targets.pop_front();
   }
 };
@@ -79,7 +79,7 @@ struct CabinAtTarget
   void operator()(const Event &, Fsm &_fsm, Source &, Target &)
   {
     const auto &data = _fsm.Data();
-    std::lock_guard<std::mutex> lock(data->system->mutex);
+    std::lock_guard<std::recursive_mutex> lock(data->system->mutex);
     data->targets.pop_front();
   }
 };

src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh

@@ -113,18 +113,18 @@ void ElevatorStateMachinePrivate::SendCmd(transport::Node::Publisher &_pub,
 }
 
 //////////////////////////////////////////////////
-ElevatorStateMachine_::ElevatorStateMachine_(
+ElevatorStateMachineDef::ElevatorStateMachineDef(
     const std::shared_ptr<ElevatorCommonPrivate> &_system)
     : dataPtr(std::make_unique<ElevatorStateMachinePrivate>(_system))
 {
 }
 
 //////////////////////////////////////////////////
-ElevatorStateMachine_::~ElevatorStateMachine_() = default;
+ElevatorStateMachineDef::~ElevatorStateMachineDef() = default;
 
 //////////////////////////////////////////////////
 const std::unique_ptr<ElevatorStateMachinePrivate>
-    &ElevatorStateMachine_::Data() const
+    &ElevatorStateMachineDef::Data() const
 {
   return this->dataPtr;
 }

src/systems/elevator/state_machine/GuardsImpl.hh

@@ -33,38 +33,47 @@ 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 <typename TargetState>
+struct IsInState
+{
+  /// \brief Function call operator
+  /// \param[in] _fsm State machine with which the guard is associated
+  public: template <typename Fsm, typename State>
+  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<TargetState()>();
+  }
+};
+
 /// \brief Guard that checks whether the cabin is at the target floor level.
-/// \note The template parameter can invert the result if set to true.
-template <bool invert>
 struct CabinAtTarget
 {
   /// \brief Function call operator
   /// \param[in] _fsm State machine with which the guard is associated
-  public: template <typename Event, typename Fsm, typename Source,
-                    typename Target>
-  bool operator()(const Event &, Fsm &_fsm, Source &, Target &)
+  public: template <typename Fsm, typename State>
+  bool operator()(const Fsm &_fsm, const State &)
   {
     const auto &data = _fsm.Data();
-    std::lock_guard<std::mutex> lock(data->system->mutex);
-    bool at_target = data->targets.front() == data->system->state;
-    return at_target ^ invert;
+    std::lock_guard<std::recursive_mutex> lock(data->system->mutex);
+    return data->targets.front() == data->system->state;
   }
 };
 
 /// \brief Guard that checks whether the target queue is empty.
-/// \note The template parameter can invert the result if set to true.
-template <bool invert>
 struct NoQueuedTarget
 {
   /// \brief Function call operator
   /// \param[in] _fsm State machine with which the guard is associated
-  public: template <typename Event, typename Fsm, typename Source,
-                    typename Target>
-  bool operator()(const Event &, Fsm &_fsm, Source &, Target &)
+  public: template <typename Fsm, typename State>
+  bool operator()(const Fsm &_fsm, const State &)
   {
     const auto &data = _fsm.Data();
-    std::lock_guard<std::mutex> lock(data->system->mutex);
-    return data->targets.empty() ^ invert;
+    std::lock_guard<std::recursive_mutex> lock(data->system->mutex);
+    return data->targets.empty();
   }
 };