laserscan_conversions.h
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 #ifndef PSEN_SCAN_V2_STANDALONE_LASERSCAN_CONVERSIONS_H
17 #define PSEN_SCAN_V2_STANDALONE_LASERSCAN_CONVERSIONS_H
18 
23 
25 
26 #include <algorithm>
27 #include <numeric>
28 #include <vector>
29 
31 {
32 namespace data_conversion_layer
33 {
37 class ScannerProtocolViolationError : public std::runtime_error
38 {
39 public:
40  ScannerProtocolViolationError(const std::string& msg) : std::runtime_error(msg){};
41 };
42 
47 {
48 public:
61  static LaserScan
62  toLaserScan(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
63 
64 private:
65  static std::vector<int> getFilledFramesIndicesSortedByThetaAngle(
66  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
67  static util::TenthOfDegree
68  calculateMaxAngle(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
69  const util::TenthOfDegree& min_angle);
70  static int64_t
71  calculateTimestamp(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
72  const std::vector<int>& filled_stamped_msgs_indices);
73  static int64_t calculateFirstRayTime(const data_conversion_layer::monitoring_frame::MessageStamped& stamped_msg);
74  static void
75  validateMonitoringFrames(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
76  const std::vector<int>& sorted_stamped_msgs_indices);
77  static bool
78  allResolutionsMatch(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
79  static bool
80  allScanCountersMatch(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
81  static bool
82  thetaAnglesFitTogether(const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
83  const std::vector<int>& sorted_stamped_msgs_indices);
84 };
85 
87  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
88 {
89  if (stamped_msgs.empty())
90  {
91  throw ScannerProtocolViolationError("At least one monitoring frame is necessary to create a LaserScan");
92  }
93 
94  std::vector<int> sorted_stamped_msgs_indices = getFilledFramesIndicesSortedByThetaAngle(stamped_msgs);
95  validateMonitoringFrames(stamped_msgs, sorted_stamped_msgs_indices);
96 
97  const auto min_angle = stamped_msgs[sorted_stamped_msgs_indices[0]].msg_.fromTheta();
98  const auto max_angle = calculateMaxAngle(stamped_msgs, min_angle);
99 
100  const auto timestamp = calculateTimestamp(stamped_msgs, sorted_stamped_msgs_indices);
101 
102  std::vector<double> measurements;
103  std::vector<double> intensities;
104  std::vector<IOState> io_states;
105 
106  for (auto index : sorted_stamped_msgs_indices)
107  {
108  measurements.insert(measurements.end(),
109  stamped_msgs[index].msg_.measurements().begin(),
110  stamped_msgs[index].msg_.measurements().end());
111  if (stamped_msgs[index].msg_.hasIntensitiesField())
112  {
113  intensities.insert(intensities.end(),
114  stamped_msgs[index].msg_.intensities().begin(),
115  stamped_msgs[index].msg_.intensities().end());
116  }
117  }
118 
119  // Issue #320: Only for the io_states, we follow reception order instead Theta order.
120  // Other wise: index=0 who is the bigger Theta and correspond to the first io_state of the
121  // frame is placed at last item of vector io_states, and it provokes that io_states flicks.
122  for (const auto& single_msg : stamped_msgs)
123  {
124  if (single_msg.msg_.hasIOPinField())
125  {
126  PSENSCAN_DEBUG("io_states: ",
127  "stamp_: {} fromTheta: {} ioPinDate: {} ",
128  single_msg.stamp_,
129  std::to_string(single_msg.msg_.fromTheta().toRad()),
130  util::formatRange(single_msg.msg_.iOPinData().input_state));
131 
132  io_states.emplace_back(single_msg.msg_.iOPinData(), single_msg.stamp_);
133  }
134  }
135 
136  LaserScan scan(stamped_msgs[0].msg_.resolution(),
137  min_angle,
138  max_angle,
139  stamped_msgs[0].msg_.scanCounter(),
140  stamped_msgs[sorted_stamped_msgs_indices.back()].msg_.activeZoneset(),
141  timestamp);
142 
143  scan.measurements(measurements);
144  scan.intensities(intensities);
145  scan.ioStates(io_states);
146 
147  return scan;
148 }
149 
151  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
152 {
153  std::vector<int> sorted_filled_stamped_msgs_indices(stamped_msgs.size());
154  std::iota(sorted_filled_stamped_msgs_indices.begin(), sorted_filled_stamped_msgs_indices.end(), 0);
155  std::sort(sorted_filled_stamped_msgs_indices.begin(),
156  sorted_filled_stamped_msgs_indices.end(),
157  [&stamped_msgs](int i1, int i2) {
158  return stamped_msgs[i1].msg_.fromTheta() < stamped_msgs[i2].msg_.fromTheta();
159  });
160 
161  // The following contains a missing line in the coverage report, which does not make sense.
162  // LCOV_EXCL_START
163  sorted_filled_stamped_msgs_indices.erase(
164  std::remove_if(sorted_filled_stamped_msgs_indices.begin(),
165  sorted_filled_stamped_msgs_indices.end(),
166  [&stamped_msgs](int i) { return stamped_msgs[i].msg_.measurements().empty(); }),
167  sorted_filled_stamped_msgs_indices.end());
168  // LCOV_EXCL_STOP
169 
170  return sorted_filled_stamped_msgs_indices;
171 }
172 
174  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
175  const util::TenthOfDegree& min_angle)
176 {
177  const auto resolution = stamped_msgs[0].msg_.resolution();
178  const uint16_t number_of_samples = std::accumulate(
179  stamped_msgs.begin(), stamped_msgs.end(), uint16_t{ 0 }, [](uint16_t total, const auto& stamped_msg) {
180  return total + stamped_msg.msg_.measurements().size();
181  });
182  return min_angle + resolution * static_cast<int>(number_of_samples - 1);
183 }
184 
186  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
187  const std::vector<int>& filled_stamped_msgs_indices)
188 {
189  const auto it = std::min_element(
190  filled_stamped_msgs_indices.begin(), filled_stamped_msgs_indices.end(), [&stamped_msgs](int i, int j) {
191  return stamped_msgs[i].stamp_ < stamped_msgs[j].stamp_;
192  }); // determines stamped_msg with smallest stamp
193  return calculateFirstRayTime(stamped_msgs[*it]);
194 }
195 
196 inline int64_t
198 {
199  const double time_per_scan_in_ns{ configuration::TIME_PER_SCAN_IN_S * 1000000000.0 };
200  const double scan_interval_in_degree{ stamped_msg.msg_.resolution().value() *
201  (stamped_msg.msg_.measurements().size() - 1) / 10.0 };
202  return stamped_msg.stamp_ - static_cast<int64_t>(std::round(scan_interval_in_degree * time_per_scan_in_ns / 360.0));
203 }
204 
206  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
207  const std::vector<int>& sorted_stamped_msgs_indices)
208 {
209  if (!allResolutionsMatch(stamped_msgs))
210  {
211  throw ScannerProtocolViolationError("The resolution of all monitoring frames has to be the same.");
212  }
213  else if (!allScanCountersMatch(stamped_msgs))
214  {
215  throw ScannerProtocolViolationError("The scan counters of all monitoring frames have to be the same.");
216  }
217  else if (!thetaAnglesFitTogether(stamped_msgs, sorted_stamped_msgs_indices))
218  {
219  throw ScannerProtocolViolationError("The monitoring frame ranges do not cover the whole scan range");
220  }
221 }
222 
224  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
225 {
226  const auto resolution = stamped_msgs[0].msg_.resolution();
227  return std::all_of(stamped_msgs.begin(), stamped_msgs.end(), [resolution](const auto& stamped_msg) {
228  return stamped_msg.msg_.resolution() == resolution;
229  });
230 }
231 
233  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
234 {
235  const auto scan_counter = stamped_msgs[0].msg_.scanCounter();
236  return std::all_of(stamped_msgs.begin(), stamped_msgs.end(), [scan_counter](const auto& stamped_msg) {
237  return stamped_msg.msg_.scanCounter() == scan_counter;
238  });
239 }
240 
242  const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
243  const std::vector<int>& sorted_filled_stamped_msgs_indices)
244 {
245  util::TenthOfDegree last_end = stamped_msgs[sorted_filled_stamped_msgs_indices[0]].msg_.fromTheta();
246  for (auto index : sorted_filled_stamped_msgs_indices)
247  {
248  const auto& stamped_msg = stamped_msgs[index];
249  if (last_end != stamped_msg.msg_.fromTheta())
250  {
251  return false;
252  }
253  last_end = stamped_msg.msg_.fromTheta() +
254  stamped_msg.msg_.resolution() * static_cast<int>(stamped_msg.msg_.measurements().size());
255  }
256  return true;
257 }
258 
259 } // namespace data_conversion_layer
260 } // namespace psen_scan_v2_standalone
261 
262 #endif // PSEN_SCAN_V2_STANDALONE_LASERSCAN_CONVERSIONS_H
static LaserScan toLaserScan(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs)
Converts monitoring_frames of a scan_round to the user friendly LaserScan type sent by the IScanner::...
static constexpr double TIME_PER_SCAN_IN_S
static bool allResolutionsMatch(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs)
static int64_t calculateFirstRayTime(const data_conversion_layer::monitoring_frame::MessageStamped &stamped_msg)
const MeasurementData & measurements() const
Definition: laserscan.cpp:133
#define PSENSCAN_DEBUG(name,...)
Definition: logging.h:63
Wrapping class for a Message and its corresponding timestamp.
static util::TenthOfDegree calculateMaxAngle(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs, const util::TenthOfDegree &min_angle)
Root namespace in which the software components to communicate with the scanner (firmware-version: 2)...
Definition: udp_client.h:41
static int64_t calculateTimestamp(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs, const std::vector< int > &filled_stamped_msgs_indices)
: Responsible for converting Monitoring frames into LaserScan messages.
: Exception thrown if data received from the scanner hardware could not be processed according to pro...
static void validateMonitoringFrames(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs, const std::vector< int > &sorted_stamped_msgs_indices)
static bool thetaAnglesFitTogether(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs, const std::vector< int > &sorted_stamped_msgs_indices)
static std::vector< int > getFilledFramesIndicesSortedByThetaAngle(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs)
std::string formatRange(const T &range)
Definition: format_range.h:38
static bool allScanCountersMatch(const std::vector< data_conversion_layer::monitoring_frame::MessageStamped > &stamped_msgs)
This class represents a single laser scan in the <tf_prefix> target frame.
Definition: laserscan.h:46
Helper class representing angles in tenth of degree.


psen_scan_v2
Author(s): Pilz GmbH + Co. KG
autogenerated on Sat Nov 5 2022 02:13:36