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"
#include "psen_scan_v2_standalone/configuration/scanner_ids.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)
{
  // Condition to fix the bug of the first scanCounter data of the Subscriber0
  if (first_scan_round_ &&
      stamped_msg.msg_.scannerId() == psen_scan_v2_standalone::configuration::ScannerId::subscriber0)
  {
    first_scan_round_ = false;
  }
  else
  {
    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