Merge branch 'development' of https://github.com/robocomp/cortex into…

… development
robocomp · Jun 3, 2024 · 99e420b · 99e420b
2 parents e033f2e + 3d5e7e6
commit 99e420b
Show file tree

Hide file tree

Showing 6 changed files with 113 additions and 24 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,7 +8,7 @@ include(GNUInstallDirs)
 
 add_definitions(-I/usr/include/x86_64-linux-gnu/qt6/QtOpenGLWidgets/)
 
-
+include_directories(/home/robocomp/robocomp/classes)
 add_subdirectory(api)
 add_subdirectory(core)
 add_subdirectory(gui)

diff --git a/README.md b/README.md
@@ -136,6 +136,8 @@ You need the following third-party software:
       sudo make install
 ```
 
+If it does not install properly, try installing it from the source: https://fast-dds.docs.eprosima.com/en/latest/installation/binaries/binaries_linux.html
+
 Update the system cache of dynamic libraries with 
 
 ```sh

diff --git a/api/dsr_rt_api.cpp b/api/dsr_rt_api.cpp
@@ -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())
@@ -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())

diff --git a/api/dsr_utils.cpp b/api/dsr_utils.cpp
@@ -6,7 +6,6 @@
 #include <QJsonDocument>
 #include <QJsonObject>
 #include <QJsonArray>
-#include <fstream>
 #include <dsr/api/dsr_utils.h>
 #include <dsr/api/dsr_api.h>
 

diff --git a/api/include/dsr/api/dsr_rt_api.h b/api/include/dsr/api/dsr_rt_api.h
@@ -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)

diff --git a/core/topics/IDLGraphPubSubTypes.cxx b/core/topics/IDLGraphPubSubTypes.cxx
@@ -1554,7 +1554,7 @@ bool MvregEdgePubSubType::getKey(
 IDLNodePubSubType::IDLNodePubSubType()
 {
     setName("IDLNode");
-    uint32_t type_size = IDLNode_max_cdr_typesize;
+    uint32_t type_size = std::numeric_limits<uint32_t>::max();
     type_size += static_cast<uint32_t>(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */
     m_typeSize = type_size + 4; /*encapsulation*/
     m_isGetKeyDefined = false;
@@ -1858,7 +1858,7 @@ bool GraphRequestPubSubType::getKey(
 DotKernelPubSubType::DotKernelPubSubType()
 {
     setName("DotKernel");
-    uint32_t type_size = DotKernel_max_cdr_typesize;
+    uint32_t type_size =  std::numeric_limits<uint32_t>::max();
     type_size += static_cast<uint32_t>(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */
     m_typeSize = type_size + 4; /*encapsulation*/
     m_isGetKeyDefined = false;
@@ -2010,7 +2010,7 @@ bool DotKernelPubSubType::getKey(
 MvregNodePubSubType::MvregNodePubSubType()
 {
     setName("MvregNode");
-    uint32_t type_size = MvregNode_max_cdr_typesize;
+    uint32_t type_size =  std::numeric_limits<uint32_t>::max();
     type_size += static_cast<uint32_t>(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */
     m_typeSize = type_size + 4; /*encapsulation*/
     m_isGetKeyDefined = false;
@@ -2162,7 +2162,7 @@ bool MvregNodePubSubType::getKey(
 OrMapPubSubType::OrMapPubSubType()
 {
     setName("OrMap");
-    uint32_t type_size = OrMap_max_cdr_typesize;
+    uint32_t type_size =  std::numeric_limits<uint32_t>::max();
     type_size += static_cast<uint32_t>(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */
     m_typeSize = type_size + 4; /*encapsulation*/
     m_isGetKeyDefined = false;