monitoring_frame_deserialization.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020-2022 Pilz GmbH & Co. KG
2 //
3 // This program is free software: you can redistribute it and/or modify
4 // it under the terms of the GNU Lesser General Public License as published by
5 // the Free Software Foundation, either version 3 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public License
14 // along with this program. If not, see <https://www.gnu.org/licenses/>.
15 
16 #include <array>
17 #include <functional>
18 #include <istream>
19 #include <sstream>
20 #include <vector>
21 
22 #include <fmt/format.h>
23 
33 
35 {
36 namespace data_conversion_layer
37 {
38 namespace monitoring_frame
39 {
40 using namespace std::placeholders;
41 
43 {
44 }
45 
47  OpCode op_code,
48  WorkingMode working_mode,
49  TransactionType transaction_type,
50  configuration::ScannerId scanner_id,
51  FromTheta from_theta,
52  Resolution resolution)
53  : device_status_(device_status)
54  , op_code_(op_code)
55  , working_mode_(working_mode)
56  , transaction_type_(transaction_type)
57  , scanner_id_(scanner_id)
58  , from_theta_(from_theta)
59  , resolution_(resolution)
60 {
61 }
62 
63 static constexpr double toMeter(const uint16_t& value)
64 {
65  if ((value == NO_SIGNAL_ARRIVED) || (value == SIGNAL_TOO_LATE))
66  {
67  return std::numeric_limits<double>::infinity();
68  }
69  return static_cast<double>(value) / 1000.;
70 }
71 
72 static constexpr double toIntensities(const uint16_t& value)
73 {
74  // Neglegt the first two bytes.
75  uint16_t retval{ value };
76  return static_cast<double>(retval & 0b0011111111111111);
77 }
78 
79 monitoring_frame::Message deserialize(const data_conversion_layer::RawData& data, const std::size_t& num_bytes)
80 {
82 
83  std::stringstream ss;
84  ss.write(data.data(), num_bytes);
85 
86  FixedFields frame_header = readFixedFields(ss);
87 
88  msg_builder.scannerId(frame_header.scannerId());
89  msg_builder.fromTheta(frame_header.fromTheta());
90  msg_builder.resolution(frame_header.resolution());
91 
92  bool end_of_frame{ false };
93  while (!end_of_frame)
94  {
95  const AdditionalFieldHeader additional_header{ readAdditionalField(ss, num_bytes) };
96  switch (static_cast<AdditionalFieldHeaderID>(additional_header.id()))
97  {
99  if (additional_header.length() != NUMBER_OF_BYTES_SCAN_COUNTER)
100  {
101  throw AdditionalFieldUnexpectedSize(fmt::format("Length of scan counter field is {}, but should be {}.",
102  additional_header.length(),
104  }
105  uint32_t scan_counter_read_buffer;
106  raw_processing::read<uint32_t>(ss, scan_counter_read_buffer);
107  msg_builder.scanCounter(scan_counter_read_buffer);
108  break;
109 
111  const size_t num_measurements{ static_cast<size_t>(additional_header.length()) /
113  std::vector<double> measurements;
114  raw_processing::readArray<uint16_t, double>(ss, measurements, num_measurements, toMeter);
115  msg_builder.measurements(measurements);
116  break;
117  }
119  end_of_frame = true;
120  break;
121 
123  if (additional_header.length() != NUMBER_OF_BYTES_ZONE_SET)
124  {
125  throw AdditionalFieldUnexpectedSize(fmt::format("Length of zone set field is {}, but should be {}.",
126  additional_header.length(),
128  }
129  uint8_t zone_set_read_buffer;
130  raw_processing::read<uint8_t>(ss, zone_set_read_buffer);
131  msg_builder.activeZoneset(zone_set_read_buffer);
132  break;
133 
135  if (additional_header.length() != io::RAW_CHUNK_LENGTH_IN_BYTES)
136  {
137  throw AdditionalFieldUnexpectedSize(fmt::format("Length of io state field is {}, but should be {}.",
138  additional_header.length(),
140  }
141  msg_builder.iOPinData(io::deserializePins(ss));
142  break;
143 
146  break;
147 
149  const size_t num_measurements{ static_cast<size_t>(additional_header.length()) /
151  std::vector<double> intensities;
152  raw_processing::readArray<uint16_t, double>(ss, intensities, num_measurements, std::bind(toIntensities, _1));
153  msg_builder.intensities(intensities);
154  break;
155  }
156  default:
157  throw DecodingFailure(
158  fmt::format("Header Id {:#04x} unknown. Cannot read additional field of monitoring frame on position {}.",
159  additional_header.id(),
160  ss.tellp()));
161  }
162  }
163  return msg_builder.build();
164 }
165 
166 AdditionalFieldHeader readAdditionalField(std::istream& is, const std::size_t& max_num_bytes)
167 {
168  auto const id = raw_processing::read<AdditionalFieldHeader::Id>(is);
169  auto length = raw_processing::read<AdditionalFieldHeader::Length>(is);
170 
171  if (length >= max_num_bytes)
172  {
173  throw DecodingFailure(
174  fmt::format("Length given in header of additional field is too large: {}, id: {:#04x}", length, id));
175  }
176  if (length > 0)
177  {
178  length--;
179  }
180  return AdditionalFieldHeader(id, length);
181 }
182 
183 namespace io
184 {
185 PinData deserializePins(std::istream& is)
186 {
188 
191 
192  raw_processing::read<std::array<uint8_t, RAW_CHUNK_LENGTH_RESERVED_IN_BYTES>>(is);
193  deserializePinField(is, io_pin_data.input_state);
194 
195  raw_processing::read<std::array<uint8_t, RAW_CHUNK_LENGTH_RESERVED_IN_BYTES>>(is);
196  deserializePinField(is, io_pin_data.output_state);
197 
198  return io_pin_data;
199 }
200 } // namespace io
201 
202 namespace diagnostic
203 {
204 std::vector<diagnostic::Message> deserializeMessages(std::istream& is)
205 {
206  std::vector<diagnostic::Message> diagnostic_messages;
207 
208  // Read-in unused data fields
209  raw_processing::read<std::array<uint8_t, diagnostic::RAW_CHUNK_UNUSED_OFFSET_IN_BYTES>>(is);
210 
211  for (const auto& scanner_id : configuration::VALID_SCANNER_IDS)
212  {
213  for (size_t byte_n = 0; byte_n < diagnostic::RAW_CHUNK_LENGTH_FOR_ONE_DEVICE_IN_BYTES; byte_n++)
214  {
215  const auto raw_byte = raw_processing::read<uint8_t>(is);
216  const std::bitset<8> raw_bits(raw_byte);
217 
218  for (size_t bit_n = 0; bit_n < raw_bits.size(); ++bit_n)
219  {
220  if (raw_bits.test(bit_n) && (diagnostic::ErrorType::unused != diagnostic::ERROR_BITS[byte_n][bit_n]))
221  {
222  diagnostic_messages.push_back(diagnostic::Message(static_cast<configuration::ScannerId>(scanner_id),
223  diagnostic::ErrorLocation(byte_n, bit_n)));
224  }
225  }
226  }
227  }
228  return diagnostic_messages;
229 }
230 } // namespace diagnostic
231 
232 FixedFields readFixedFields(std::istream& is)
233 {
234  const auto device_status = raw_processing::read<FixedFields::DeviceStatus>(is);
235  const auto op_code = raw_processing::read<FixedFields::OpCode>(is);
236  const auto working_mode = raw_processing::read<FixedFields::WorkingMode>(is);
237  const auto transaction_type = raw_processing::read<FixedFields::TransactionType>(is);
238  const auto scanner_id = raw_processing::read<configuration::ScannerId>(is);
239 
240  const auto from_theta = raw_processing::read<int16_t, FixedFields::FromTheta>(is);
241  const auto resolution = raw_processing::read<int16_t, FixedFields::Resolution>(is);
242 
243  // LCOV_EXCL_START
244  if (OP_CODE_MONITORING_FRAME != op_code)
245  {
247  0.1, "monitoring_frame::Message", "Unexpected opcode during deserialization of MonitoringFrame.");
248  }
249 
250  if (ONLINE_WORKING_MODE != working_mode)
251  {
252  PSENSCAN_ERROR_THROTTLE(0.1, "monitoring_frame::Message", "Invalid working mode (not online)");
253  }
254 
255  if (GUI_MONITORING_TRANSACTION != transaction_type)
256  {
257  PSENSCAN_ERROR_THROTTLE(0.1, "monitoring_frame::Message", "Invalid transaction type.");
258  }
259 
260  if (MAX_SCANNER_ID < static_cast<uint8_t>(scanner_id))
261  {
262  PSENSCAN_ERROR_THROTTLE(0.1, "monitoring_frame::Message", "Invalid Scanner id.");
263  }
264  // LCOV_EXCL_STOP
265 
266  return FixedFields(device_status, op_code, working_mode, transaction_type, scanner_id, from_theta, resolution);
267 }
268 } // namespace monitoring_frame
269 } // namespace data_conversion_layer
270 } // namespace psen_scan_v2_standalone
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::diagnostic::ErrorLocation
Defines a byte and bit position of an error in the diagnostic chunk.
Definition: diagnostics.h:148
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::DeviceStatus
uint32_t DeviceStatus
Definition: monitoring_frame_deserialization.h:70
psen_scan_v2_standalone::configuration::ScannerId
ScannerId
Definition: scanner_ids.h:27
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::readAdditionalField
AdditionalFieldHeader readAdditionalField(std::istream &is, const std::size_t &max_num_bytes)
Definition: monitoring_frame_deserialization.cpp:166
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::resolution
MessageBuilder & resolution(const util::TenthOfDegree &resolution)
Definition: monitoring_frame_msg_builder.h:76
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::scan_counter
@ scan_counter
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::OP_CODE_MONITORING_FRAME
static constexpr uint32_t OP_CODE_MONITORING_FRAME
Definition: monitoring_frame_deserialization.h:54
monitoring_frame_msg.h
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::DecodingFailure
Exception thrown on problems during the extraction of the measurement data.
Definition: monitoring_frame_deserialization.h:172
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::io::deserializePins
PinData deserializePins(std::istream &is)
Definition: monitoring_frame_deserialization.cpp:185
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::measurements
MessageBuilder & measurements(const std::vector< double > &measurements)
Definition: monitoring_frame_msg_builder.h:82
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::io::RAW_CHUNK_LENGTH_IN_BYTES
static constexpr uint32_t RAW_CHUNK_LENGTH_IN_BYTES
Definition: io_constants.h:38
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::NO_SIGNAL_ARRIVED
static constexpr uint16_t NO_SIGNAL_ARRIVED
Definition: monitoring_frame_deserialization.h:60
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::io::PinData
Represents the IO PIN field of a monitoring frame.
Definition: io_pin_data.h:72
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::ONLINE_WORKING_MODE
static constexpr uint32_t ONLINE_WORKING_MODE
Definition: monitoring_frame_deserialization.h:55
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::NUMBER_OF_BYTES_SCAN_COUNTER
static constexpr uint16_t NUMBER_OF_BYTES_SCAN_COUNTER
Definition: monitoring_frame_deserialization.h:57
psen_scan_v2_standalone::configuration::VALID_SCANNER_IDS
static constexpr std::array< ScannerId, 4 > VALID_SCANNER_IDS
Definition: scanner_ids.h:35
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::resolution
Resolution resolution() const
Definition: monitoring_frame_deserialization.h:244
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder
Definition: monitoring_frame_msg_builder.h:33
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::io::deserializePinField
void deserializePinField(std::istream &is, std::array< std::bitset< 8 >, ChunkSize > &pin_states)
Definition: monitoring_frame_deserialization.h:158
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldUnexpectedSize
Exception thrown on problems with the additional fields with fixed size.
Definition: monitoring_frame_deserialization.h:190
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeader::Length
uint16_t Length
Definition: monitoring_frame_deserialization.h:119
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::intensities
MessageBuilder & intensities(const std::vector< double > &intensities)
Definition: monitoring_frame_msg_builder.h:100
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::diagnostic::ERROR_BITS
static constexpr std::array< std::array< ErrorType, 8 >, 9 > ERROR_BITS
Definition: diagnostics.h:123
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::end_of_frame
@ end_of_frame
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::activeZoneset
MessageBuilder & activeZoneset(uint8_t active_zoneset)
Definition: monitoring_frame_msg_builder.h:94
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::readFixedFields
FixedFields readFixedFields(std::istream &is)
Definition: monitoring_frame_deserialization.cpp:232
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::NUMBER_OF_BYTES_ZONE_SET
static constexpr uint16_t NUMBER_OF_BYTES_ZONE_SET
Definition: monitoring_frame_deserialization.h:58
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::scannerId
configuration::ScannerId scannerId() const
Definition: monitoring_frame_deserialization.h:234
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::diagnostic::RAW_CHUNK_LENGTH_FOR_ONE_DEVICE_IN_BYTES
static constexpr uint32_t RAW_CHUNK_LENGTH_FOR_ONE_DEVICE_IN_BYTES
Contains constants and types needed to define the diagnostic::Message.
Definition: diagnostics.h:45
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::scannerId
MessageBuilder & scannerId(configuration::ScannerId scanner_id)
Definition: monitoring_frame_msg_builder.h:64
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::scanCounter
MessageBuilder & scanCounter(uint32_t scan_counter)
Definition: monitoring_frame_msg_builder.h:88
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeader
Definition for the type and length of an additional field in a monitoring frame.
Definition: monitoring_frame_deserialization.h:115
raw_scanner_data.h
diagnostics.h
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::build
Message build()
Definition: monitoring_frame_msg_builder.h:54
PSENSCAN_ERROR_THROTTLE
#define PSENSCAN_ERROR_THROTTLE(period, name,...)
Definition: logging.h:74
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID
AdditionalFieldHeaderID
Definition: monitoring_frame_deserialization.h:135
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::diagnostic::Message
Defines an Diagnostic message by defining the ErrorLocation and a scanner ID.
Definition: diagnostics.h:181
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeader::Id
uint8_t Id
Definition: monitoring_frame_deserialization.h:118
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::WorkingMode
uint32_t WorkingMode
Definition: monitoring_frame_deserialization.h:72
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::FixedFields
FixedFields(DeviceStatus device_status, OpCode op_code, WorkingMode working_mode, TransactionType transaction_type, configuration::ScannerId scanner_id, FromTheta from_theta, Resolution resolution)
Definition: monitoring_frame_deserialization.cpp:46
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::GUI_MONITORING_TRANSACTION
static constexpr uint32_t GUI_MONITORING_TRANSACTION
Definition: monitoring_frame_deserialization.h:56
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MAX_SCANNER_ID
static constexpr uint8_t MAX_SCANNER_ID
Definition: monitoring_frame_msg.h:44
raw_processing.h
io_state.h
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::iOPinData
MessageBuilder & iOPinData(const io::PinData &io_pin_data)
Definition: monitoring_frame_msg_builder.h:112
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::fromTheta
FromTheta fromTheta() const
Definition: monitoring_frame_deserialization.h:239
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::io::RAW_CHUNK_PHYSICAL_INPUT_SIGNALS_IN_BYTES
static constexpr uint32_t RAW_CHUNK_PHYSICAL_INPUT_SIGNALS_IN_BYTES
Definition: io_constants.h:35
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::diagnostics
@ diagnostics
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::diagnostic::ErrorType::unused
@ unused
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeader::AdditionalFieldHeader
AdditionalFieldHeader(Id id, Length length)
Definition: monitoring_frame_deserialization.cpp:42
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::deserialize
monitoring_frame::Message deserialize(const data_conversion_layer::RawData &data, const std::size_t &num_bytes)
Definition: monitoring_frame_deserialization.cpp:79
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
logging.h
psen_scan_v2_standalone
Root namespace in which the software components to communicate with the scanner (firmware-version: 2)...
Definition: udp_client.h:41
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::io_pin_data
@ io_pin_data
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::TransactionType
uint32_t TransactionType
Definition: monitoring_frame_deserialization.h:73
monitoring_frame_msg_builder.h
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::diagnosticMessages
MessageBuilder & diagnosticMessages(const std::vector< diagnostic::Message > &diagnostic_messages)
Definition: monitoring_frame_msg_builder.h:106
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::Message
Higher level data type representing a single monitoring frame.
Definition: monitoring_frame_msg.h:62
monitoring_frame_deserialization.h
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields::OpCode
uint32_t OpCode
Definition: monitoring_frame_deserialization.h:71
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::NUMBER_OF_BYTES_SINGLE_MEASUREMENT
static constexpr uint16_t NUMBER_OF_BYTES_SINGLE_MEASUREMENT
Definition: monitoring_frame_deserialization.h:59
psen_scan_v2_standalone::data_conversion_layer::raw_processing::read
void read(std::istream &is, T &data)
Definition: raw_processing.h:53
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::toIntensities
static constexpr double toIntensities(const uint16_t &value)
Definition: monitoring_frame_deserialization.cpp:72
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::io::RAW_CHUNK_LENGTH_RESERVED_IN_BYTES
static constexpr uint32_t RAW_CHUNK_LENGTH_RESERVED_IN_BYTES
Contains constants and types needed to define PinData and for the (de)serialization.
Definition: io_constants.h:34
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::MessageBuilder::fromTheta
MessageBuilder & fromTheta(const util::TenthOfDegree &from_theta)
Definition: monitoring_frame_msg_builder.h:70
psen_scan_v2_standalone::data_conversion_layer::RawData
std::vector< char > RawData
Definition: raw_scanner_data.h:25
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::SIGNAL_TOO_LATE
static constexpr uint16_t SIGNAL_TOO_LATE
Definition: monitoring_frame_deserialization.h:61
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::measurements
@ measurements
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::intensities
@ intensities
psen_scan_v2_standalone::util::TenthOfDegree
Helper class representing angles in tenth of degree.
Definition: tenth_of_degree.h:34
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::diagnostic::deserializeMessages
std::vector< diagnostic::Message > deserializeMessages(std::istream &is)
Definition: monitoring_frame_deserialization.cpp:204
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::FixedFields
The information included in every single monitoring frame.
Definition: monitoring_frame_deserialization.h:67
io_pin_data.h
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::AdditionalFieldHeaderID::zone_set
@ zone_set
psen_scan_v2_standalone::data_conversion_layer::monitoring_frame::toMeter
static constexpr double toMeter(const uint16_t &value)
Definition: monitoring_frame_deserialization.cpp:63


psen_scan_v2
Author(s): Pilz GmbH + Co. KG
autogenerated on Sat Jun 22 2024 02:46:11