16 #ifndef PSEN_SCAN_V2_STANDALONE_LASERSCAN_CONVERSIONS_H
17 #define PSEN_SCAN_V2_STANDALONE_LASERSCAN_CONVERSIONS_H
32 namespace data_conversion_layer
62 toLaserScan(
const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
66 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
68 calculateMaxAngle(
const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
71 calculateTimestamp(
const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
72 const std::vector<int>& filled_stamped_msgs_indices);
76 const std::vector<int>& sorted_stamped_msgs_indices);
78 allResolutionsMatch(
const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
80 allScanCountersMatch(
const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs);
82 thetaAnglesFitTogether(
const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
83 const std::vector<int>& sorted_stamped_msgs_indices);
87 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
89 if (stamped_msgs.empty())
97 const auto min_angle = stamped_msgs[sorted_stamped_msgs_indices[0]].msg_.fromTheta();
102 std::vector<double> measurements;
103 std::vector<double> intensities;
104 std::vector<IOState> io_states;
106 for (
auto index : sorted_stamped_msgs_indices)
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())
113 intensities.insert(intensities.end(),
114 stamped_msgs[index].msg_.intensities().begin(),
115 stamped_msgs[index].msg_.intensities().end());
122 for (
const auto& single_msg : stamped_msgs)
124 if (single_msg.msg_.hasIOPinField())
127 "stamp_: {} fromTheta: {} ioPinDate: {} ",
129 std::to_string(single_msg.msg_.fromTheta().toRad()),
132 io_states.emplace_back(single_msg.msg_.iOPinData(), single_msg.stamp_);
136 LaserScan scan(stamped_msgs[0].msg_.resolution(),
139 stamped_msgs[0].msg_.scanCounter(),
140 stamped_msgs[sorted_stamped_msgs_indices.back()].msg_.activeZoneset(),
151 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
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();
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());
170 return sorted_filled_stamped_msgs_indices;
174 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
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();
182 return min_angle + resolution *
static_cast<int>(number_of_samples - 1);
186 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
187 const std::vector<int>& filled_stamped_msgs_indices)
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_;
202 return stamped_msg.
stamp_ -
static_cast<int64_t
>(std::round(scan_interval_in_degree * time_per_scan_in_ns / 360.0));
206 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
207 const std::vector<int>& sorted_stamped_msgs_indices)
224 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
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;
233 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs)
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;
242 const std::vector<data_conversion_layer::monitoring_frame::MessageStamped>& stamped_msgs,
243 const std::vector<int>& sorted_filled_stamped_msgs_indices)
245 util::TenthOfDegree last_end = stamped_msgs[sorted_filled_stamped_msgs_indices[0]].msg_.fromTheta();
246 for (
auto index : sorted_filled_stamped_msgs_indices)
248 const auto& stamped_msg = stamped_msgs[index];
249 if (last_end != stamped_msg.msg_.fromTheta())
253 last_end = stamped_msg.msg_.fromTheta() +
254 stamped_msg.msg_.resolution() *
static_cast<int>(stamped_msg.msg_.measurements().size());
262 #endif // PSEN_SCAN_V2_STANDALONE_LASERSCAN_CONVERSIONS_H