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 <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"

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_{};
  ScanBuffer scan_buffer_{ DEFAULT_NUM_MSG_PER_ROUND };
  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