scan_buffer.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_SCAN_ROUND_H
#define PSEN_SCAN_V2_STANDALONE_SCAN_ROUND_H

#include <exception>

#include "psen_scan_v2_standalone/data_conversion_layer/monitoring_frame_msg.h"
#include "psen_scan_v2_standalone/util/logging.h"

namespace psen_scan_v2_standalone
{
namespace protocol_layer
{
class ScanRoundError : public std::runtime_error
{
public:
  ScanRoundError(const std::string& msg) : std::runtime_error(msg){};
};

class OutdatedMessageError : public ScanRoundError
{
public:
  OutdatedMessageError(const std::string& msg = "Detected a MonitoringFrame from an earlier round. "
                                                " The scan round will ignore it.")
    : ScanRoundError(msg){};
};

class ScanRoundEndedEarlyError : public ScanRoundError
{
public:
  ScanRoundEndedEarlyError(const std::string& msg = "Detected a MonitoringFrame from a new scan round before the old "
                                                    "one was complete."
                                                    " Dropping the incomplete round."
                                                    " (Please check the ethernet connection or contact PILZ support if "
                                                    "the error persists.)")
    : ScanRoundError(msg){};
};

class ScanRoundOversaturatedError : public ScanRoundError
{
public:
  ScanRoundOversaturatedError(const std::string& msg = "Received too many MonitoringFrames for one scan round.")
    : ScanRoundError(msg){};
};

class ScanBuffer
{
public:
  ScanBuffer(const uint32_t& num_expected_msgs);
  void add(const data_conversion_layer::monitoring_frame::MessageStamped& stamped_msg);

  void reset();
  std::vector<data_conversion_layer::monitoring_frame::MessageStamped> currentRound();

  bool isRoundComplete();

private:
  void startNewRound(const data_conversion_layer::monitoring_frame::MessageStamped& stamped_msg);

private:
  std::vector<data_conversion_layer::monitoring_frame::MessageStamped> current_round_{};
  const uint32_t& num_expected_msgs_;
  bool first_scan_round_ = true;
};

inline ScanBuffer::ScanBuffer(const uint32_t& num_expected_msgs) : num_expected_msgs_(num_expected_msgs)
{
}

inline void ScanBuffer::reset()
{
  current_round_.clear();
}

inline std::vector<data_conversion_layer::monitoring_frame::MessageStamped> ScanBuffer::currentRound()
{
  return current_round_;
}

inline bool ScanBuffer::isRoundComplete()
{
  return current_round_.size() == num_expected_msgs_;
}

inline void ScanBuffer::add(const data_conversion_layer::monitoring_frame::MessageStamped& stamped_msg)
{
  if (current_round_.empty() || stamped_msg.msg_.scanCounter() == current_round_[0].msg_.scanCounter())
  {
    current_round_.push_back(stamped_msg);
    if (current_round_.size() > num_expected_msgs_)
    {
      throw ScanRoundOversaturatedError();
    }
  }
  else if (stamped_msg.msg_.scanCounter() > current_round_[0].msg_.scanCounter())
  {
    startNewRound(stamped_msg);
  }
  else
  {
    throw OutdatedMessageError();
  }
}

inline void ScanBuffer::startNewRound(const data_conversion_layer::monitoring_frame::MessageStamped& stamped_msg)
{
  bool old_round_undersaturated = current_round_.size() < num_expected_msgs_;
  reset();
  current_round_.push_back(stamped_msg);
  if (old_round_undersaturated && !first_scan_round_)
  {
    throw ScanRoundEndedEarlyError();
  }
  first_scan_round_ = false;
}
}  // namespace protocol_layer
}  // namespace psen_scan_v2_standalone

#endif  // PSEN_SCAN_V2_STANDALONE_SCAN_ROUND_H