Skip to content

Commit

Permalink
[RT API] use history in insert_or_assign_edge_RT
Browse files Browse the repository at this point in the history
  • Loading branch information
@JuanCarlosgg
JuanCarlosgg committed May 25, 2024
1 parent 604c2a5 commit 130f851
Show file tree
Hide file tree
Showing 3 changed files with 106 additions and 19 deletions.
115 changes: 101 additions & 14 deletions api/dsr_rt_api.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,13 +174,57 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, const std::vector<fl
std::unique_lock<std::shared_mutex> lock(G->_mutex);
if (G->nodes.contains(to))
{
CRDTEdge e; e.to(to); e.from(n.id()); e.type("RT"); e.agent_id(G->agent_id);
CRDTAttribute tr(trans, get_unix_timestamp(), 0);
CRDTAttribute rot(rot_euler, get_unix_timestamp(), 0);
auto [it, new_el] = e.attrs().emplace("rt_rotation_euler_xyz", mvreg<CRDTAttribute> ());
it->second.write(std::move(rot));
auto [it2, new_el2] = e.attrs().emplace("rt_translation", mvreg<CRDTAttribute> ());
it2->second.write(std::move(tr));
CRDTEdge e;
if (HISTORY_SIZE <= 0)
{
e.to(to); e.from(n.id()); e.type("RT"); e.agent_id(G->agent_id);
CRDTAttribute tr(trans, get_unix_timestamp(), 0);
CRDTAttribute rot(rot_euler, get_unix_timestamp(), 0);

auto [it, new_el] = e.attrs().emplace("rt_rotation_euler_xyz", mvreg<CRDTAttribute> ());
it->second.write(std::move(rot));
auto [it2, new_el2] = e.attrs().emplace("rt_translation", mvreg<CRDTAttribute> ());
it2->second.write(std::move(tr));
} else {

e = G->get_edge_(n.id(), to, "RT").value_or(CRDTEdge());
e.to(to); e.from(n.id()); e.type("RT"); e.agent_id(G->agent_id);
auto head_o = G->get_attrib_by_name<rt_head_index_att>(e);
std::optional<std::vector<uint64_t>> tstamps_o = G->get_attrib_by_name<rt_timestamps_att>(e);
std::optional<std::vector<float>> tr_pack_o = G->get_attrib_by_name<rt_translation_att>(e);
std::optional<std::vector<float>> rot_pack_o = G->get_attrib_by_name<rt_rotation_euler_xyz_att>(e);
auto time_stamps = tstamps_o.value_or(std::vector<std::uint64_t>(HISTORY_SIZE, 0));
auto tr_pack = tr_pack_o.value_or(std::vector<float> (BLOCK_SIZE * HISTORY_SIZE, 0.f));
auto rot_pack = rot_pack_o.value_or(std::vector<float> (BLOCK_SIZE * HISTORY_SIZE, 0.f));

auto timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE+1) % HISTORY_SIZE;
uint32_t index = timestamp_index * BLOCK_SIZE;

tr_pack[index] = trans[0];
tr_pack[index + 1] = trans[1];
tr_pack[index + 2] = trans[2];
rot_pack[index] = rot_euler[0];
rot_pack[index + 1] = rot_euler[1];
rot_pack[index + 2] = rot_euler[2];
time_stamps[timestamp_index] = static_cast<std::uint64_t>(
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count());


CRDTAttribute tr(std::move(tr_pack), get_unix_timestamp(), 0);
CRDTAttribute rot(std::move(rot_pack), get_unix_timestamp(), 0);
CRDTAttribute head_index(index, get_unix_timestamp(), 0);
CRDTAttribute timestamps(std::move(time_stamps), get_unix_timestamp(), 0);

auto [it, new_el] = e.attrs().insert_or_assign("rt_rotation_euler_xyz", mvreg<CRDTAttribute> ());
it->second.write(std::move(rot));
std::tie(it, new_el) = e.attrs().insert_or_assign("rt_translation", mvreg<CRDTAttribute> ());
it->second.write(std::move(tr));
std::tie(it, new_el) = e.attrs().insert_or_assign("rt_head_index", mvreg<CRDTAttribute> ());
it->second.write(std::move(head_index));
std::tie(it, new_el) = e.attrs().insert_or_assign("rt_timestamps", mvreg<CRDTAttribute> ());
it->second.write(std::move(timestamps));
}

to_n = G->get_(to).value();
if (auto x = G->get_crdt_attrib_by_name<parent_att>(to_n.value()); x.has_value())
Expand Down Expand Up @@ -274,14 +318,57 @@ void RT_API::insert_or_assign_edge_RT(Node &n, uint64_t to, std::vector<float> &
std::unique_lock<std::shared_mutex> lock(G->_mutex);
if (G->nodes.contains(to))
{
CRDTEdge e; e.to(to); e.from(n.id()); e.type("RT"); e.agent_id(G->agent_id);
CRDTAttribute tr; tr.value(std::move(trans)); tr.timestamp(get_unix_timestamp());
CRDTAttribute rot; rot.value(std::move(rot_euler)); rot.timestamp(get_unix_timestamp());
auto [it, new_el] = e.attrs().emplace("rt_rotation_euler_xyz", mvreg<CRDTAttribute> ());
it->second.write(std::move(rot));
auto [it2, new_el2] = e.attrs().emplace("rt_translation", mvreg<CRDTAttribute> ());
it2->second.write(std::move(tr));
CRDTEdge e;
if (HISTORY_SIZE <= 0)
{
e.to(to); e.from(n.id()); e.type("RT"); e.agent_id(G->agent_id);
CRDTAttribute tr(std::move(trans), get_unix_timestamp(), 0);
CRDTAttribute rot(std::move(rot_euler), get_unix_timestamp(), 0);

auto [it, new_el] = e.attrs().emplace("rt_rotation_euler_xyz", mvreg<CRDTAttribute> ());
it->second.write(std::move(rot));
auto [it2, new_el2] = e.attrs().emplace("rt_translation", mvreg<CRDTAttribute> ());
it2->second.write(std::move(tr));
} else {

e = G->get_edge_(n.id(), to, "RT").value_or(CRDTEdge());
e.to(to); e.from(n.id()); e.type("RT"); e.agent_id(G->agent_id);
auto head_o = G->get_attrib_by_name<rt_head_index_att>(e);
std::optional<std::vector<uint64_t>> tstamps_o = G->get_attrib_by_name<rt_timestamps_att>(e);
std::optional<std::vector<float>> tr_pack_o = G->get_attrib_by_name<rt_translation_att>(e);
std::optional<std::vector<float>> rot_pack_o = G->get_attrib_by_name<rt_rotation_euler_xyz_att>(e);
auto time_stamps = tstamps_o.value_or(std::vector<std::uint64_t>(HISTORY_SIZE, 0));
auto tr_pack = tr_pack_o.value_or(std::vector<float> (BLOCK_SIZE * HISTORY_SIZE, 0.f));
auto rot_pack = rot_pack_o.value_or(std::vector<float> (BLOCK_SIZE * HISTORY_SIZE, 0.f));

auto timestamp_index = (int)(head_o.value_or(0)/BLOCK_SIZE+1) % HISTORY_SIZE;
uint32_t index = timestamp_index * BLOCK_SIZE;

tr_pack[index] = trans[0];
tr_pack[index + 1] = trans[1];
tr_pack[index + 2] = trans[2];
rot_pack[index] = rot_euler[0];
rot_pack[index + 1] = rot_euler[1];
rot_pack[index + 2] = rot_euler[2];
time_stamps[timestamp_index] = static_cast<std::uint64_t>(
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count());


CRDTAttribute tr(std::move(tr_pack), get_unix_timestamp(), 0);
CRDTAttribute rot(std::move(rot_pack), get_unix_timestamp(), 0);
CRDTAttribute head_index(index, get_unix_timestamp(), 0);
CRDTAttribute timestamps(std::move(time_stamps), get_unix_timestamp(), 0);

auto [it, new_el] = e.attrs().insert_or_assign("rt_rotation_euler_xyz", mvreg<CRDTAttribute> ());
it->second.write(std::move(rot));
std::tie(it, new_el) = e.attrs().insert_or_assign("rt_translation", mvreg<CRDTAttribute> ());
it->second.write(std::move(tr));
std::tie(it, new_el) = e.attrs().insert_or_assign("rt_head_index", mvreg<CRDTAttribute> ());
it->second.write(std::move(head_index));
std::tie(it, new_el) = e.attrs().insert_or_assign("rt_timestamps", mvreg<CRDTAttribute> ());
it->second.write(std::move(timestamps));
}

to_n = G->get_(to).value();
if (auto x = G->get_crdt_attrib_by_name<parent_att>(to_n.value()); x.has_value())
Expand Down
1 change: 0 additions & 1 deletion api/dsr_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <fstream>
#include <dsr/api/dsr_utils.h>
#include <dsr/api/dsr_api.h>

Expand Down
9 changes: 5 additions & 4 deletions api/include/dsr/api/dsr_rt_api.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,15 +18,16 @@ namespace DSR
public:
explicit RT_API(DSRGraph *G_);

const int BLOCK_SIZE = 3; // size of 3-vector for translation and euler xyz angles
const int32_t BLOCK_SIZE = 3; // size of 3-vector for translation and euler xyz angles
uint32_t HISTORY_SIZE = 0; // Number of blocks in the history.

void insert_or_assign_edge_RT(Node &n, uint64_t to, const std::vector<float> &trans, const std::vector<float> &rot_euler);
void insert_or_assign_edge_RT(Node &n, uint64_t to, std::vector<float> &&trans, std::vector<float> &&rot_euler);
std::optional<Edge> get_edge_RT(const Node &n, uint64_t to);
std::optional<Mat::RTMat> get_RT_pose_from_parent(const Node &n);
std::optional<Mat::RTMat> get_edge_RT_as_rtmat(const Edge &edge, std::uint64_t timestamp = -1);
std::optional<Eigen::Vector3d> get_translation(const Node &n, uint64_t to, std::uint64_t timestamp = -1);
std::optional<Eigen::Vector3d> get_translation(uint64_t node_id, uint64_t to, std::uint64_t timestamp = -1);
std::optional<Mat::RTMat> get_edge_RT_as_rtmat(const Edge &edge, std::uint64_t timestamp = 0);
std::optional<Eigen::Vector3d> get_translation(const Node &n, uint64_t to, std::uint64_t timestamp = 0);
std::optional<Eigen::Vector3d> get_translation(uint64_t node_id, uint64_t to, std::uint64_t timestamp = 0);
// std::optional<Mat::RTMat> get_edge_RT_as_rtmat(const Node &n, uint32_t to);
// std::optional<std::tuple<Mat::Vector3d, Mat::Quaterniond>> get_edge_RT_as_tr_plus_quaternion(const Edge &edge);
// std::optional<Mat::MatXX> get_jacobian(const Node &base, const Node &tip)
Expand Down

0 comments on commit 130f851

Please sign in to comment.