run_permitted_state_machine.h

Go to the documentation of this file.

/*
 * Copyright (c) 2019 Pilz GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H
#define PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H

#include <queue>
#include <string>

#include <ros/ros.h>

#include <boost/msm/back/state_machine.hpp>
#include <boost/msm/front/functor_row.hpp>
#include <boost/msm/front/state_machine_def.hpp>
#include <boost/core/demangle.hpp>
#include <ros/console.h>

#include <prbt_hardware_support/service_function_decl.h>
#include <prbt_hardware_support/utils.h>

namespace prbt_hardware_support
{
#define COLOR_GREEN "\033[32m"
#define COLOR_GREEN_BOLD "\033[1;32m"

#define STATE_ENTER_OUTPUT                                                                                             \
  ROS_DEBUG_STREAM_NAMED("RunPermittedStateMachine",                                                                   \
                         "Event: " << className(boost::core::demangle(typeid(ev).name()))                              \
                                   << " - Entering: " << COLOR_GREEN_BOLD                                              \
                                   << className(boost::core::demangle(typeid(*this).name())) << COLOR_GREEN);
#define STATE_EXIT_OUTPUT                                                                                              \
  ROS_DEBUG_STREAM_NAMED("RunPermittedStateMachine",                                                                   \
                         "Event: " << className(boost::core::demangle(typeid(ev).name()))                              \
                                   << " - Leaving: " << className(boost::core::demangle(typeid(*this).name())));
#define ACTION_OUTPUT                                                                                                  \
  ROS_DEBUG_STREAM_NAMED("RunPermittedStateMachine",                                                                   \
                         "Event: " << className(boost::core::demangle(typeid(ev).name()))                              \
                                   << " - Action: " << className(boost::core::demangle(typeid(*this).name())));

class AsyncRunPermittedTask
{
public:
  AsyncRunPermittedTask(const TServiceCallFunc& operation, const std::function<void()>& finished_handler)
    : operation_(operation), finished_handler_(finished_handler)
  {
  }

  void execute()
  {
    if (operation_)
    {
      operation_();
    }
  }

  void signalCompletion()
  {
    finished_handler_();
  }

private:
  TServiceCallFunc operation_;
  std::function<void()> finished_handler_;
};

using RunPermittedTaskQueue = std::queue<AsyncRunPermittedTask>;

namespace msm = boost::msm;
namespace mpl = boost::mpl;
using namespace msm::front;

class RunPermittedStateMachine_ : public msm::front::state_machine_def<RunPermittedStateMachine_>  // CRTP
{
public:
  RunPermittedStateMachine_(const TServiceCallFunc& recover_operation, const TServiceCallFunc& halt_operation,
                            const TServiceCallFunc& hold_operation, const TServiceCallFunc& unhold_operation)
    : recover_op_(recover_operation), halt_op_(halt_operation), hold_op_(hold_operation), unhold_op_(unhold_operation)
  {
  }

  // States //

  struct RobotInactive : public msm::front::state<>
  {
    template <class Event, class FSM>
    void on_entry(Event const& ev, FSM&)
    {
      STATE_ENTER_OUTPUT
    }
    template <class Event, class FSM>
    void on_exit(Event const& ev, FSM&)
    {
      STATE_EXIT_OUTPUT
    }
  };
  struct RobotActive : public msm::front::state<>
  {
    template <class Event, class FSM>
    void on_entry(Event const& ev, FSM&)
    {
      STATE_ENTER_OUTPUT
    }
    template <class Event, class FSM>
    void on_exit(Event const& ev, FSM&)
    {
      STATE_EXIT_OUTPUT
    }
  };

  struct Enabling : public msm::front::state<>
  {
    template <class Event, class FSM>
    void on_entry(Event const& ev, FSM&)
    {
      STATE_ENTER_OUTPUT
    }
    template <class Event, class FSM>
    void on_exit(Event const& ev, FSM&)
    {
      STATE_EXIT_OUTPUT
    }
  };

  struct Stopping : public msm::front::state<>
  {
    template <class Event, class FSM>
    void on_entry(Event const& ev, FSM&)
    {
      STATE_ENTER_OUTPUT
    }
    template <class Event, class FSM>
    void on_exit(Event const& ev, FSM&)
    {
      STATE_EXIT_OUTPUT
    }
  };

  struct StopRequestedDuringEnable : public msm::front::state<>
  {
    template <class Event, class FSM>
    void on_entry(Event const& ev, FSM&)
    {
      STATE_ENTER_OUTPUT
    }
    template <class Event, class FSM>
    void on_exit(Event const& ev, FSM&)
    {
      STATE_EXIT_OUTPUT
    }
  };

  struct EnableRequestedDuringStop : public msm::front::state<>
  {
    template <class Event, class FSM>
    void on_entry(Event const& ev, FSM&)
    {
      STATE_ENTER_OUTPUT
    }
    template <class Event, class FSM>
    void on_exit(Event const& ev, FSM&)
    {
      STATE_EXIT_OUTPUT
    }
  };

  typedef RobotInactive initial_state;

  // Events //

  struct run_permitted_updated
  {
    run_permitted_updated(const bool run_permitted) : run_permitted_(run_permitted)
    {
    }

    bool run_permitted_;
  };

  struct recover_done
  {
  };

  struct halt_done
  {
  };

  struct hold_done
  {
  };

  struct unhold_done
  {
  };

  // Guards //

  struct run_permitted_true
  {
    template <class EVT, class FSM, class SourceState, class TargetState>
    bool operator()(EVT const& evt, FSM&, SourceState&, TargetState&)
    {
      return evt.run_permitted_;
    }
  };

  struct run_permitted_false
  {
    template <class EVT, class FSM, class SourceState, class TargetState>
    bool operator()(EVT const& evt, FSM&, SourceState&, TargetState&)
    {
      return !evt.run_permitted_;
    }
  };

  // Actions //

  struct recover_start
  {
    template <class EVT, class FSM, class SourceState, class TargetState>
    void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
    {
      ACTION_OUTPUT

      fsm.task_queue_.push(AsyncRunPermittedTask(fsm.recover_op_, [&fsm]() { fsm.process_event(recover_done()); }));
    }
  };

  struct halt_start
  {
    template <class EVT, class FSM, class SourceState, class TargetState>
    void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
    {
      ACTION_OUTPUT

      fsm.task_queue_.push(AsyncRunPermittedTask(fsm.halt_op_, [&fsm]() { fsm.process_event(halt_done()); }));
    }
  };

  struct hold_start
  {
    template <class EVT, class FSM, class SourceState, class TargetState>
    void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
    {
      ACTION_OUTPUT

      fsm.task_queue_.push(AsyncRunPermittedTask(fsm.hold_op_, [&fsm]() { fsm.process_event(hold_done()); }));
    }
  };

  struct unhold_start
  {
    template <class EVT, class FSM, class SourceState, class TargetState>
    void operator()(EVT const& ev, FSM& fsm, SourceState&, TargetState&)
    {
      ACTION_OUTPUT

      fsm.task_queue_.push(AsyncRunPermittedTask(fsm.unhold_op_, [&fsm]() { fsm.process_event(unhold_done()); }));
    }
  };

  // Transitions //

  struct transition_table
    : mpl::vector<
          //  Start                       Event          Target                      Action         Guard
          // +---------------------------+--------------+---------------------------+--------------+----------+
          Row<RobotInactive, run_permitted_updated, Enabling, recover_start, run_permitted_true>,
          Row<RobotInactive, run_permitted_updated, none, none, run_permitted_false>,
          Row<Enabling, run_permitted_updated, none, none, run_permitted_true>,
          Row<Enabling, run_permitted_updated, StopRequestedDuringEnable, none, run_permitted_false>,
          Row<Enabling, recover_done, none, unhold_start, none>, Row<Enabling, unhold_done, RobotActive, none, none>,
          Row<StopRequestedDuringEnable, run_permitted_updated, none, none, none>,
          Row<StopRequestedDuringEnable, recover_done, Stopping, halt_start, none>,
          Row<StopRequestedDuringEnable, unhold_done, Stopping, hold_start, none>,
          Row<RobotActive, run_permitted_updated, none, none, run_permitted_true>,
          Row<RobotActive, run_permitted_updated, Stopping, hold_start, run_permitted_false>,
          Row<Stopping, run_permitted_updated, EnableRequestedDuringStop, none, run_permitted_true>,
          Row<Stopping, hold_done, none, halt_start, none>, Row<Stopping, halt_done, RobotInactive, none, none>,
          Row<EnableRequestedDuringStop, hold_done, none, halt_start, none>,
          Row<EnableRequestedDuringStop, halt_done, Enabling, recover_start, none>,
          Row<EnableRequestedDuringStop, run_permitted_updated, Stopping, none, run_permitted_false>,
          Row<EnableRequestedDuringStop, run_permitted_updated, none, none, run_permitted_true>
          // +---------------------------+--------------+---------------------------+--------------+----------+
          >
  {
  };

  RunPermittedTaskQueue task_queue_;

  TServiceCallFunc recover_op_;

  TServiceCallFunc halt_op_;

  TServiceCallFunc hold_op_;

  TServiceCallFunc unhold_op_;
};

typedef msm::back::state_machine<RunPermittedStateMachine_> RunPermittedStateMachine;

}  // namespace prbt_hardware_support

#endif  // PRBT_HARDWARE_SUPPORT_RUN_PERMITTED_STATE_MACHINE_H