scanner_state_machine.h

Go to the documentation of this file.

// Copyright (c) 2020-2021 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 <https://www.gnu.org/licenses/>.
#ifndef PSEN_SCAN_V2_STANDALONE_SCANNER_PROTOCOL_DEF_H
#define PSEN_SCAN_V2_STANDALONE_SCANNER_PROTOCOL_DEF_H
 
#include <functional>
#include <string>
#include <memory>
#include <mutex>
#include <chrono>
#include <stdexcept>
#include <unordered_map>
#include <vector>
#include <boost/optional.hpp>
 
#define BOOST_MSM_CONSTRUCTOR_ARG_SIZE 12  // see https://www.boost.org/doc/libs/1_66_0/libs/msm/doc/HTML/ch03s05.html
 
// back-end
#include <boost/msm/back/state_machine.hpp>
// front-end
#include <boost/msm/front/state_machine_def.hpp>
 
#include <boost/msm/back/tools.hpp>
#include <boost/msm/back/metafunctions.hpp>
 
#include "psen_scan_v2_standalone/protocol_layer/scanner_events.h"
 
#include "psen_scan_v2_standalone/util/logging.h"
#include "psen_scan_v2_standalone/util/format_range.h"
#include "psen_scan_v2_standalone/util/ip_conversion.h"
#include "psen_scan_v2_standalone/communication_layer/udp_client.h"
 
#include "psen_scan_v2_standalone/laserscan.h"
#include "psen_scan_v2_standalone/data_conversion_layer/laserscan_conversions.h"
 
#include "psen_scan_v2_standalone/data_conversion_layer/start_request.h"
#include "psen_scan_v2_standalone/data_conversion_layer/stop_request_serialization.h"
 
#include "psen_scan_v2_standalone/data_conversion_layer/scanner_reply_msg.h"
#include "psen_scan_v2_standalone/data_conversion_layer/scanner_reply_serialization_deserialization.h"
#include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h"
#include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_deserialization.h"
#include "psen_scan_v2_standalone/protocol_layer/scan_buffer.h"
#include "psen_scan_v2_standalone/util/watchdog.h"
#include "psen_scan_v2_standalone/configuration/scanner_ids.h"
 
namespace psen_scan_v2_standalone
{
namespace protocol_layer
{
namespace msm = boost::msm;
namespace mpl = boost::mpl;
 
namespace e = psen_scan_v2_standalone::protocol_layer::scanner_events;
 
// clang-format off
#define STATE(state_name)\
  class state_name : public msm::front::state<>\
{\
  public:\
  template <class Event, class FSM>\
  void on_entry(Event const&, FSM& fsm);\
  \
  template <class Event, class FSM>\
  void on_exit(Event const&, FSM& fsm);\
}
// clang-format on
 
static constexpr std::chrono::milliseconds WATCHDOG_TIMEOUT{ 1000 };
static constexpr uint32_t DEFAULT_NUM_MSG_PER_ROUND{ 6 };
 
using ScannerStartedCallback = std::function<void()>;
using ScannerStoppedCallback = std::function<void()>;
using StartErrorCallback = std::function<void(const std::string&)>;
using StopErrorCallback = std::function<void(const std::string&)>;
using TimeoutCallback = std::function<void()>;
using InformUserAboutLaserScanCallback = std::function<void(const LaserScan&)>;
 
class IWatchdogFactory
{
public:
  virtual ~IWatchdogFactory() = default;
 
public:
  virtual std::unique_ptr<util::Watchdog> create(const util::Watchdog::Timeout& timeout,
                                                 const TimeoutCallback& timeout_callback) = 0;
};
 
class WatchdogFactory : public IWatchdogFactory
{
public:
  WatchdogFactory() = default;
  std::unique_ptr<util::Watchdog> create(const util::Watchdog::Timeout& timeout,
                                         const TimeoutCallback& timeout_callback) override;
};
 
// front-end: define the FSM structure
class ScannerProtocolDef : public msm::front::state_machine_def<ScannerProtocolDef>
{
public:
  ScannerProtocolDef(const ScannerConfiguration& config,
                     const communication_layer::NewMessageCallback& control_msg_callback,
                     const communication_layer::ErrorCallback& control_error_callback,
                     const communication_layer::ErrorCallback& start_error_callback,
                     const communication_layer::ErrorCallback& stop_error_callback,
                     const communication_layer::NewMessageCallback& data_msg_callback,
                     const communication_layer::ErrorCallback& data_error_callback,
                     const ScannerStartedCallback& scanner_started_callback,
                     const ScannerStoppedCallback& scanner_stopped_callback,
                     const InformUserAboutLaserScanCallback& laser_scan_callback,
                     const TimeoutCallback& start_timeout_callback,
                     const TimeoutCallback& monitoring_frame_timeout_callback);
 
public:  // States
  STATE(Idle);
  STATE(WaitForStartReply);
  STATE(WaitForMonitoringFrame);
  STATE(WaitForStopReply);
  STATE(Stopped);
  STATE(Error);
 
public:  // Action methods
  template <class T>
  void sendStartRequest(const T& event);
  void handleStartRequestTimeout(const scanner_events::StartTimeout& event);
  template <class T>
  void sendStopRequest(const T& event);
  void handleMonitoringFrame(const scanner_events::RawMonitoringFrameReceived& event);
  void handleMonitoringFrameTimeout(const scanner_events::MonitoringFrameTimeout& event);
  void notifyUserAboutStart(scanner_events::RawReplyReceived const& reply_event);
  void notifyUserAboutUnknownStartReply(scanner_events::RawReplyReceived const& reply_event);
  void notifyUserAboutRefusedStartReply(scanner_events::RawReplyReceived const& reply_event);
  void notifyUserAboutStop(scanner_events::RawReplyReceived const& reply_event);
  void notifyUserAboutUnknownStopReply(scanner_events::RawReplyReceived const& reply_event);
  void notifyUserAboutRefusedStopReply(scanner_events::RawReplyReceived const& reply_event);
 
public:  // Guards
  bool isAcceptedStopReply(scanner_events::RawReplyReceived const& reply_event);
  bool isUnknownStopReply(scanner_events::RawReplyReceived const& reply_event);
  bool isRefusedStopReply(scanner_events::RawReplyReceived const& reply_event);
  bool isAcceptedStartReply(scanner_events::RawReplyReceived const& reply_event);
  bool isUnknownStartReply(scanner_events::RawReplyReceived const& reply_event);
  bool isRefusedStartReply(scanner_events::RawReplyReceived const& reply_event);
 
public:  // Replaces the default exception/no-transition responses
  template <class FSM, class Event>
  void exception_caught(Event const& event, FSM& fsm, std::exception& exception);  // NOLINT
 
  template <class FSM, class Event>
  void no_transition(Event const& event, FSM& /*unused*/, int state);  // NOLINT
 
  template <class FSM>
  void
  no_transition(const scanner_events::RawMonitoringFrameReceived& /*unused*/, FSM& /*unused*/, int state);  // NOLINT
 
public:  // Definition of state machine via table
  typedef Idle initial_state;
  typedef ScannerProtocolDef m;
 
  // clang-format off
  struct transition_table : mpl::vector<  // NOLINT
      //    Start                         Event                         Next                        Action                        Guard
      //  +------------------------------+----------------------------+---------------------------+--------------------------------------+-------------------------+
      a_row  < Idle,                      e::StartRequest,              WaitForStartReply,          &m::sendStartRequest                                           >,
      a_row  < Idle,                      e::StopRequest,               WaitForStopReply,           &m::sendStopRequest                                            >,
      row    < WaitForStartReply,         e::RawReplyReceived,          WaitForMonitoringFrame,     &m::notifyUserAboutStart,             &m::isAcceptedStartReply >,
      row    < WaitForStartReply,         e::RawReplyReceived,          Error,                      &m::notifyUserAboutRefusedStartReply, &m::isRefusedStartReply  >,
      row    < WaitForStartReply,         e::RawReplyReceived,          Error,                      &m::notifyUserAboutUnknownStartReply, &m::isUnknownStartReply  >,
      a_irow < WaitForStartReply,         e::StartTimeout,                                          &m::handleStartRequestTimeout                                  >,
      a_irow < WaitForMonitoringFrame,    e::RawMonitoringFrameReceived,                            &m::handleMonitoringFrame                                      >,
      a_irow < WaitForMonitoringFrame,    e::MonitoringFrameTimeout,                                &m::handleMonitoringFrameTimeout                               >,
      a_row  < WaitForStartReply,         e::StopRequest,               WaitForStopReply,           &m::sendStopRequest                                            >,
      a_row  < WaitForMonitoringFrame,    e::StopRequest,               WaitForStopReply,           &m::sendStopRequest                                            >,
      _irow  < WaitForStopReply,          e::RawMonitoringFrameReceived                                                                                            >,
      row    < WaitForStopReply,          e::RawReplyReceived,          Stopped,                    &m::notifyUserAboutStop,              &m::isAcceptedStopReply  >,
      row    < WaitForStopReply,          e::RawReplyReceived,          Error,                      &m::notifyUserAboutRefusedStopReply,  &m::isRefusedStopReply   >,
      row    < WaitForStopReply,          e::RawReplyReceived,          Error,                      &m::notifyUserAboutUnknownStopReply,  &m::isUnknownStopReply   >,
      _irow  < Stopped,                   e::RawMonitoringFrameReceived                                                                                            >
      //  +------------------------------+----------------------------+--------------------------+---------------------------------------+-------------------------+
      > {};
  // clang-format on
 
private:
  // LCOV_EXCL_START
  class InternalScannerReplyError : public std::runtime_error
  {
  public:
    InternalScannerReplyError(const std::string& error_msg);
  };
  // LCOV_EXCL_STOP
  bool isStartReply(data_conversion_layer::scanner_reply::Message const& msg);
  bool isStopReply(data_conversion_layer::scanner_reply::Message const& msg);
  bool isAcceptedReply(data_conversion_layer::scanner_reply::Message const& msg);
  bool isUnknownReply(data_conversion_layer::scanner_reply::Message const& msg);
  bool isRefusedReply(data_conversion_layer::scanner_reply::Message const& msg);
 
  void checkForInternalErrors(const data_conversion_layer::scanner_reply::Message& msg);
  void checkForDiagnosticErrors(const data_conversion_layer::monitoring_frame::Message& msg);
  void checkForChangedActiveZoneset(const data_conversion_layer::monitoring_frame::Message& msg);
 
  void informUserAboutTheScanData(const data_conversion_layer::monitoring_frame::MessageStamped& stamped_msg);
  void
  sendMessageWithMeasurements(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msg);
  bool
  framesContainMeasurements(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msg);
 
private:
  ScannerConfiguration config_;
 
  std::unique_ptr<util::Watchdog> start_reply_watchdog_{};
 
  std::unique_ptr<util::Watchdog> monitoring_frame_watchdog_{};
 
  using ScannerId = psen_scan_v2_standalone::configuration::ScannerId;
  std::unordered_map<ScannerId, ScanBuffer> scan_buffers_{};
 
  boost::optional<data_conversion_layer::monitoring_frame::Message> zoneset_reference_msg_;
 
  // Udp Clients
  communication_layer::UdpClientImpl control_client_;
  communication_layer::UdpClientImpl data_client_;
 
  // Callbacks
  const ScannerStartedCallback scanner_started_callback_;
  const ScannerStoppedCallback scanner_stopped_callback_;
  const StartErrorCallback start_error_callback_;
  const StopErrorCallback stop_error_callback_;
  const InformUserAboutLaserScanCallback inform_user_about_laser_scan_callback_;
 
  // Timeout Handler
  const std::function<void()> start_timeout_callback_;
  const std::function<void()> monitoring_frame_timeout_callback_;
 
  // Factories
  WatchdogFactory watchdog_factory_{};
};
 
// Pick a back-end
using ScannerStateMachine = msm::back::state_machine<ScannerProtocolDef>;
 
}  // namespace protocol_layer
}  // namespace psen_scan_v2_standalone
 
#include "psen_scan_v2_standalone/protocol_layer/scanner_state_machine_def.h"
 
#endif  // PSEN_SCAN_V2_STANDALONE_SCANNER_PROTOCOL_DEF_H