Skip to content

Commit 56bfef6

Browse files
JuanCarlosggJuanCarlosgg
committed
feat: extra fix for rt_api history
1 parent 2c6337f commit 56bfef6

File tree

1 file changed

+38
-45
lines changed

1 file changed

+38
-45
lines changed

‎api/dsr_rt_api.cpp‎

Lines changed: 38 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -202,10 +202,24 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, const std::vector<fl
202202
if (tr_pack.size() < BLOCK_SIZE * HISTORY_SIZE) tr_pack.resize(BLOCK_SIZE * HISTORY_SIZE);
203203
if (rot_pack.size() < BLOCK_SIZE * HISTORY_SIZE) rot_pack.resize(BLOCK_SIZE * HISTORY_SIZE);
204204

205-
auto timestamp_index = 0;
206-
int index = 0;
207205
bool update_index = true;
208-
if (timestamp.has_value()) {
206+
auto timestamp_index = 0;
207+
if (!head_o.has_value()) {
208+
timestamp_index = 0;
209+
} else {
210+
timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE) % HISTORY_SIZE;
211+
}
212+
int index = timestamp_index * BLOCK_SIZE;
213+
214+
auto timestamp_v = (timestamp.has_value()) ? *timestamp : static_cast<std::uint64_t>(
215+
std::chrono::duration_cast<std::chrono::milliseconds>(
216+
std::chrono::system_clock::now().time_since_epoch()).count());
217+
218+
219+
constexpr auto next = [](auto v, auto size) { return (v + 1) % size; };
220+
constexpr auto prev = [](auto v, auto size) { return (v > 0) ? v - 1 : size - 1; };
221+
222+
if (timestamp_v < time_stamps[prev(timestamp_index, HISTORY_SIZE)]) {
209223
std::vector<int64_t> diffs;
210224
std::transform(time_stamps.begin(), time_stamps.end(), std::back_inserter(diffs),
211225
[t = *timestamp](auto &val) {
@@ -214,18 +228,9 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, const std::vector<fl
214228

215229
auto pos = (((std::min_element(diffs.begin(), diffs.end())) - diffs.begin())) % HISTORY_SIZE;
216230

217-
if (!head_o.has_value()) {
218-
timestamp_index = 0;
219-
} else {
220-
timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE) % HISTORY_SIZE;
221-
}
222-
223-
index = timestamp_index * BLOCK_SIZE;
224-
225231
// too old to insert it
226-
if (pos == timestamp_index && *timestamp < time_stamps[pos]) {return;}
232+
if (pos == timestamp_index && timestamp_v < time_stamps[pos]) {return;}
227233
if (pos > timestamp_index) {
228-
pos = timestamp_index;
229234
update_index = false;
230235
}
231236

@@ -242,22 +247,14 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, const std::vector<fl
242247
time_stamps.insert(time_stamps.begin() + pos, *timestamp);
243248

244249
} else {
245-
if (!head_o.has_value()) {
246-
timestamp_index = 0;
247-
} else {
248-
timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE) % HISTORY_SIZE;
249-
}
250-
index = timestamp_index * BLOCK_SIZE;
251250

252251
tr_pack[index] = trans[0];
253252
tr_pack[index + 1] = trans[1];
254253
tr_pack[index + 2] = trans[2];
255254
rot_pack[index] = rot_euler[0];
256255
rot_pack[index + 1] = rot_euler[1];
257256
rot_pack[index + 2] = rot_euler[2];
258-
time_stamps[timestamp_index] = static_cast<std::uint64_t>(
259-
std::chrono::duration_cast<std::chrono::milliseconds>(
260-
std::chrono::system_clock::now().time_since_epoch()).count());
257+
time_stamps[timestamp_index] = timestamp_v;
261258
}
262259

263260
CRDTAttribute tr(std::move(tr_pack), get_unix_timestamp(), 0);
@@ -396,29 +393,33 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, std::vector<float> &
396393
if (tr_pack.size() < BLOCK_SIZE * HISTORY_SIZE) tr_pack.resize(BLOCK_SIZE * HISTORY_SIZE);
397394
if (rot_pack.size() < BLOCK_SIZE * HISTORY_SIZE) rot_pack.resize(BLOCK_SIZE * HISTORY_SIZE);
398395

399-
auto timestamp_index = 0;
400-
int index = 0;
401396
bool update_index = true;
402-
if (timestamp.has_value()) {
397+
auto timestamp_index = 0;
398+
if (!head_o.has_value()) {
399+
timestamp_index = 0;
400+
} else {
401+
timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE) % HISTORY_SIZE;
402+
}
403+
int index = timestamp_index * BLOCK_SIZE;
404+
auto timestamp_v = (timestamp.has_value()) ? *timestamp : static_cast<std::uint64_t>(
405+
std::chrono::duration_cast<std::chrono::milliseconds>(
406+
std::chrono::system_clock::now().time_since_epoch()).count());
407+
408+
constexpr auto next = [](auto v, auto size) { return (v + 1) % size; };
409+
constexpr auto prev = [](auto v, auto size) { return (v > 0) ? v - 1 : size - 1; };
410+
411+
if (timestamp_v < time_stamps[prev(timestamp_index, HISTORY_SIZE)]) {
403412
std::vector<int64_t> diffs;
404413
std::transform(time_stamps.begin(), time_stamps.end(), std::back_inserter(diffs),
405414
[t = *timestamp](auto &val) {
406415
return ((int64_t)t - (int64_t)val > 0) ? ((int64_t)t - (int64_t)val) : std::numeric_limits<int64_t>::min();
407416
});
408417

409418
auto pos = (((std::min_element(diffs.begin(), diffs.end())) - diffs.begin())) % HISTORY_SIZE;
410-
411-
if (!head_o.has_value()) {
412-
timestamp_index = 0;
413-
} else {
414-
timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE) % HISTORY_SIZE;
415-
}
416-
index = timestamp_index * BLOCK_SIZE;
417-
419+
418420
// too old to insert it
419-
if (pos == timestamp_index && *timestamp < time_stamps[pos]) {return;}
420-
if (pos > timestamp_index) {
421-
pos = timestamp_index;
421+
if (pos == timestamp_index && timestamp_v < time_stamps[pos]) {return;}
422+
if (pos >= timestamp_index) {
422423
update_index = false;
423424
}
424425

@@ -435,22 +436,14 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, std::vector<float> &
435436
time_stamps.insert(time_stamps.begin() + pos, *timestamp);
436437

437438
} else {
438-
if (!head_o.has_value()) {
439-
timestamp_index = 0;
440-
} else {
441-
timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE) % HISTORY_SIZE;
442-
}
443-
index = timestamp_index * BLOCK_SIZE;
444439

445440
tr_pack[index] = trans[0];
446441
tr_pack[index + 1] = trans[1];
447442
tr_pack[index + 2] = trans[2];
448443
rot_pack[index] = rot_euler[0];
449444
rot_pack[index + 1] = rot_euler[1];
450445
rot_pack[index + 2] = rot_euler[2];
451-
time_stamps[timestamp_index] = static_cast<std::uint64_t>(
452-
std::chrono::duration_cast<std::chrono::milliseconds>(
453-
std::chrono::system_clock::now().time_since_epoch()).count());
446+
time_stamps[timestamp_index] = timestamp_v;
454447
}
455448

456449

0 commit comments

Comments
 (0)