Merge branch 'main' into camintrin-phase2

agri-gaia · May 24, 2023 · b364392 · b364392
2 parents 905a5ed + e8aeadb
commit b364392
Show file tree

Hide file tree

Showing 36 changed files with 322 additions and 116 deletions.
diff --git a/.devcontainer/.vscode/launch.json b/.devcontainer/.vscode/launch.json
@@ -40,7 +40,7 @@
             "name": "seerep server",
             "type": "cppdbg",
             "request": "launch",
-            "program": "${workspaceFolder}/devel/bin/seerep-server_server",
+            "program": "${workspaceFolder}/devel/bin/seerep_server_server",
             "args": ["-c/seerep/src/seerep.cfg"],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}/seerep-data",

diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
@@ -1,46 +1,48 @@
-name: Integration Tests
+name: Tests
 
 on:
   push:
-    branches: [ main ]
-  pull_request:
-    branches: [ main ]
 
 jobs:
-  Build-and-run-tests:
+  hdf5-interface:
+    name: HDF5 Interface
     runs-on: ubuntu-latest
     container:
       image: ghcr.io/agri-gaia/seerep_base:latest
-      options: --user root
-
     steps:
-    - uses: actions/checkout@v3
+    - name: Checkout code
+      uses: actions/checkout@v3
       with:
         path: src
-
-    - name: Build the ros workspace
-      run: |
-        source /opt/ros/noetic/setup.bash
-        catkin build --workspace $GITHUB_WORKSPACE/
-      shell: bash
-
-    - name: Test FlatBuffers write/load of image
+    - name: Build seerep_hdf5 packages
       run: |
         source /opt/ros/noetic/setup.bash
+        catkin build seerep_hdf5_* --workspace $GITHUB_WORKSPACE/
         source $GITHUB_WORKSPACE/devel/setup.bash
-        $GITHUB_WORKSPACE/devel/bin/flatbuffer_write_load_test
       shell: bash
-
-    - name: Test Protobuf write/load of image
-      run: |
-        source /opt/ros/noetic/setup.bash
-        source $GITHUB_WORKSPACE/devel/setup.bash
-        $GITHUB_WORKSPACE/devel/bin/protobuf_write_load_test
+    - name: Run flatbuffer tests
+      run: catkin test seerep_hdf5_fb
+      shell: bash
+    - name: Run protobuf tests
+      run: catkin test seerep_hdf5_pb
       shell: bash
 
-    - name: Test Conversion of ROS Messages to Flatbuffer
+  ros-converions:
+    name: ROS Conversions
+    runs-on: ubuntu-latest
+    container:
+      image: ghcr.io/agri-gaia/seerep_base:latest
+    steps:
+    - name: Checkout code
+      uses: actions/checkout@v3
+      with:
+        path: src
+    - name: Build seerep_ros packages
       run: |
         source /opt/ros/noetic/setup.bash
+        catkin build seerep_ros_* --workspace $GITHUB_WORKSPACE/
         source $GITHUB_WORKSPACE/devel/setup.bash
-        catkin test seerep_ros_conversions_fb
+      shell: bash
+    - name: Test flatbuffer conversions
+      run: catkin test seerep_ros_conversions_fb
       shell: bash
diff --git a/docker/start_server.sh b/docker/start_server.sh
@@ -4,4 +4,4 @@
 source /opt/ros/noetic/setup.bash
 source /seerep/devel/setup.bash
 
-/seerep/devel/bin/seerep-server_server "$@"
+/seerep/devel/bin/seerep_server_server "$@"
diff --git a/examples/cpp/seerep_ros_communication/src/client.cpp b/examples/cpp/seerep_ros_communication/src/client.cpp
@@ -81,8 +81,8 @@ void seerep_grpc_ros::TransferSensorMsgs::send(const tf2_msgs::TFMessage::ConstP
   {
     grpc::ClientContext context;
     seerep::pb::ServerResponse response;
-    grpc::Status status =
-        stubTf->TransferTransformStamped(&context, seerep_ros_conversions_pb::toProto(tf, projectuuid), &response);
+    grpc::Status status = stubTf->TransferTransformStamped(
+        &context, seerep_ros_conversions_pb::toProto(tf, false, projectuuid), &response);
     checkStatus(status, response);
   }
 }

diff --git a/examples/cpp/seerep_ros_communication/src/clientImagesWithDetection.cpp b/examples/cpp/seerep_ros_communication/src/clientImagesWithDetection.cpp
@@ -112,7 +112,7 @@ void seerep_grpc_ros::TransferImagesWithDetection::send(const sensor_msgs::NavSa
   transform.transform.rotation.z = 0;
   transform.transform.rotation.w = 1;
 
-  if (!writerTf_->Write(seerep_ros_conversions_fb::toFlat(transform, projectuuid_)))
+  if (!writerTf_->Write(seerep_ros_conversions_fb::toFlat(transform, projectuuid_, false)))
   {
     ROS_ERROR_STREAM("error while transfering tf");
   }

diff --git a/examples/cpp/seerep_ros_communication/src/hdf5dump.cpp b/examples/cpp/seerep_ros_communication/src/hdf5dump.cpp
@@ -101,7 +101,7 @@ void DumpSensorMsgs::dump(const tf2_msgs::TFMessage::ConstPtr& msg) const
   {
     try
     {
-      m_ioTf->writeTransformStamped(seerep_ros_conversions_pb::toProto(transform));
+      m_ioTf->writeTransformStamped(seerep_ros_conversions_pb::toProto(transform, false));
     }
     catch (const std::exception& e)
     {

diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_general.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_general.h
@@ -160,6 +160,9 @@ class Hdf5CoreGeneral
   void writeProjectFrameId(const std::string& frameId);
   std::string readProjectFrameId();
 
+  void writeVersion(const std::string& version);
+  const std::optional<std::string> readVersion();
+
   // ################
   //  Geodetic Coordinates
   // ################
@@ -245,6 +248,7 @@ class Hdf5CoreGeneral
 
   inline static const std::string PROJECTNAME = "projectname";
   inline static const std::string PROJECTFRAMEID = "projectframeid";
+  inline static const std::string VERSION = "version";
 
   inline static const std::string GEODETICLOCATION_COORDINATESYSTEM = "geoloc_coordinatesystem";
   inline static const std::string GEODETICLOCATION_ELLIPSOID = "geoloc_ellipsoid";

diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_tf.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_tf.h
@@ -21,8 +21,9 @@ class Hdf5CoreTf : public Hdf5CoreGeneral
 public:
   Hdf5CoreTf(std::shared_ptr<HighFive::File>& file, std::shared_ptr<std::mutex>& write_mtx);
 
-  std::optional<std::vector<geometry_msgs::TransformStamped>> readTransformStamped(const std::string& id);
-  std::optional<std::vector<std::string>> readTransformStampedFrames(const std::string& id);
+  std::optional<std::vector<geometry_msgs::TransformStamped>> readTransformStamped(const std::string& id,
+                                                                                   const bool isStatic);
+  std::optional<std::vector<std::string>> readTransformStampedFrames(const std::string& id, const bool isStatic);
 
 private:
   std::string readFrame(const std::string& frameName, const std::shared_ptr<const HighFive::Group>& group_ptr) const;
@@ -39,6 +40,7 @@ class Hdf5CoreTf : public Hdf5CoreGeneral
 public:
   // datatype group names in hdf5
   inline static const std::string HDF5_GROUP_TF = "tf";
+  inline static const std::string HDF5_GROUP_TF_STATIC = "tf_static";
 
   inline static const std::string SIZE = "size";
 

diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp
@@ -95,6 +95,39 @@ std::string Hdf5CoreGeneral::readProjectFrameId()
   return frameId;
 }
 
+void Hdf5CoreGeneral::writeVersion(const std::string& version)
+{
+  const std::scoped_lock lock(*m_write_mtx);
+
+  if (!m_file->hasAttribute(VERSION))
+  {
+    m_file->createAttribute<std::string>(VERSION, version);
+  }
+  else
+  {
+    m_file->getAttribute(VERSION).write(version);
+  }
+  m_file->flush();
+}
+
+const std::optional<std::string> Hdf5CoreGeneral::readVersion()
+{
+  const std::scoped_lock lock(*m_write_mtx);
+
+  std::string version;
+  try
+  {
+    version = readAttributeFromHdf5<std::string>(m_file->getName(), *m_file, VERSION);
+  }
+  catch (const std::exception& e)
+  {
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << e.what();
+    return std::nullopt;
+  }
+
+  return version;
+}
+
 std::optional<std::string> Hdf5CoreGeneral::readFrameId(const std::string& datatypeGroup, const std::string& uuid)
 {
   std::string id = datatypeGroup + "/" + uuid;

diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_tf.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_tf.cpp
@@ -9,11 +9,22 @@ Hdf5CoreTf::Hdf5CoreTf(std::shared_ptr<HighFive::File>& file, std::shared_ptr<st
 {
 }
 
-std::optional<std::vector<geometry_msgs::TransformStamped>> Hdf5CoreTf::readTransformStamped(const std::string& id)
+std::optional<std::vector<geometry_msgs::TransformStamped>> Hdf5CoreTf::readTransformStamped(const std::string& id,
+                                                                                             const bool isStatic)
 {
   const std::scoped_lock lock(*m_write_mtx);
 
-  std::string hdf5GroupPath = HDF5_GROUP_TF + "/" + id;
+  std::string hdf5_group_tf;
+  if (isStatic)
+  {
+    hdf5_group_tf = HDF5_GROUP_TF_STATIC;
+  }
+  else
+  {
+    hdf5_group_tf = HDF5_GROUP_TF;
+  }
+
+  std::string hdf5GroupPath = hdf5_group_tf + "/" + id;
   std::string hdf5DatasetTimePath = hdf5GroupPath + "/" + "time";
   std::string hdf5DatasetTransPath = hdf5GroupPath + "/" + "translation";
   std::string hdf5DatasetRotPath = hdf5GroupPath + "/" + "rotation";
@@ -46,11 +57,22 @@ std::optional<std::vector<geometry_msgs::TransformStamped>> Hdf5CoreTf::readTran
   return convertToTfs(size, parentframe, childframe, time, trans, rot);
 }
 
-std::optional<std::vector<std::string>> Hdf5CoreTf::readTransformStampedFrames(const std::string& id)
+std::optional<std::vector<std::string>> Hdf5CoreTf::readTransformStampedFrames(const std::string& id,
+                                                                               const bool isStatic)
 {
   const std::scoped_lock lock(*m_write_mtx);
 
-  std::string hdf5GroupPath = HDF5_GROUP_TF + "/" + id;
+  std::string hdf5_group_tf;
+  if (isStatic)
+  {
+    hdf5_group_tf = HDF5_GROUP_TF_STATIC;
+  }
+  else
+  {
+    hdf5_group_tf = HDF5_GROUP_TF;
+  }
+
+  std::string hdf5GroupPath = hdf5_group_tf + "/" + id;
 
   if (!m_file->exist(hdf5GroupPath))
   {

diff --git a/seerep_hdf5/seerep_hdf5_fb/CMakeLists.txt b/seerep_hdf5/seerep_hdf5_fb/CMakeLists.txt
@@ -25,6 +25,8 @@ find_package(
   REQUIRED
 )
 
+find_package(catkin REQUIRED COMPONENTS geometry_msgs)
+
 enable_testing()
 find_package(GTest 1.12.0 REQUIRED)
 
@@ -38,6 +40,7 @@ include_directories(
   ${FLATBUFFER_INCLUDE_DIR}
   ${HighFive_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
 )
 
 add_library(

diff --git a/seerep_hdf5/seerep_hdf5_fb/cmake/SeerepHdf5FbConfig.cmake.in b/seerep_hdf5/seerep_hdf5_fb/cmake/SeerepHdf5FbConfig.cmake.in
@@ -28,9 +28,9 @@ find_package(SeerepHdf5Core REQUIRED)
 list(APPEND SeerepHdf5Fb_INCLUDE_DIRS ${SeerepHdf5Core_INCLUDE_DIRS})
 list(APPEND SeerepHdf5Fb_LIBRARIES ${SeerepHdf5Core_LIBRARIES})
 
-find_package(SeerepCore REQUIRED)
-list(APPEND SeerepHdf5Fb_INCLUDE_DIRS ${SeerepCore_INCLUDE_DIRS})
-list(APPEND SeerepHdf5Fb_LIBRARIES ${SeerepCore_LIBRARIES})
+#find_package(SeerepCore REQUIRED)
+#list(APPEND SeerepHdf5Fb_INCLUDE_DIRS ${SeerepCore_INCLUDE_DIRS})
+#list(APPEND SeerepHdf5Fb_LIBRARIES ${SeerepCore_LIBRARIES})
 
 # find_package(catkin REQUIRED COMPONENTS sensor_msgs)
 # list(APPEND SeerepHdf5Fb_INCLUDE_DIRS ${catkin_INCLUDE_DIRS})

diff --git a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_tf.h b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_tf.h
@@ -26,8 +26,8 @@ class Hdf5FbTf : public Hdf5FbGeneral
   void writeTransformStamped(const seerep::fb::TransformStamped& tf);
 
   std::optional<std::vector<flatbuffers::Offset<seerep::fb::TransformStamped>>>
-  readTransformStamped(const std::string& id);
-  std::optional<std::vector<std::string>> readTransformStampedFrames(const std::string& id);
+  readTransformStamped(const std::string& id, const bool isStatic);
+  std::optional<std::vector<std::string>> readTransformStampedFrames(const std::string& id, const bool isStatic);
 };
 
 }  // namespace seerep_hdf5_fb

diff --git a/seerep_hdf5/seerep_hdf5_fb/package.xml b/seerep_hdf5/seerep_hdf5_fb/package.xml
@@ -17,6 +17,7 @@
   <depend>seerep_msgs</depend>
   <depend>seerep_com</depend>
   <depend>seerep_hdf5_core</depend>
+  <depend>geometry_msgs</depend>
 
   <buildtool_depend>cmake</buildtool_depend>
 

diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp
@@ -15,8 +15,17 @@ void Hdf5FbTf::writeTransformStamped(const seerep::fb::TransformStamped& tf)
 {
   const std::scoped_lock lock(*m_write_mtx);
 
-  std::string hdf5DatasetPath = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF + "/" + tf.header()->frame_id()->str() +
-                                "_" + tf.child_frame_id()->str();
+  std::string hdf5_group_tf;
+  if (tf.is_static())
+  {
+    hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF_STATIC;
+  }
+  else
+  {
+    hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF;
+  }
+
+  std::string hdf5DatasetPath = hdf5_group_tf + "/" + tf.header()->frame_id()->str() + "_" + tf.child_frame_id()->str();
   std::string hdf5DatasetTimePath = hdf5DatasetPath + "/" + "time";
   std::string hdf5DatasetTransPath = hdf5DatasetPath + "/" + "translation";
   std::string hdf5DatasetRotPath = hdf5DatasetPath + "/" + "rotation";
@@ -112,11 +121,21 @@ void Hdf5FbTf::writeTransformStamped(const seerep::fb::TransformStamped& tf)
 }
 
 std::optional<std::vector<flatbuffers::Offset<seerep::fb::TransformStamped>>>
-Hdf5FbTf::readTransformStamped(const std::string& id)
+Hdf5FbTf::readTransformStamped(const std::string& id, const bool isStatic)
 {
   const std::scoped_lock lock(*m_write_mtx);
 
-  std::string hdf5GroupPath = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF + "/" + id;
+  std::string hdf5_group_tf;
+  if (isStatic)
+  {
+    hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF_STATIC;
+  }
+  else
+  {
+    hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF;
+  }
+
+  std::string hdf5GroupPath = hdf5_group_tf + "/" + id;
   std::string hdf5DatasetTimePath = hdf5GroupPath + "/" + "time";
   std::string hdf5DatasetTransPath = hdf5GroupPath + "/" + "translation";
   std::string hdf5DatasetRotPath = hdf5GroupPath + "/" + "rotation";
@@ -191,11 +210,21 @@ Hdf5FbTf::readTransformStamped(const std::string& id)
   return tfs;
 }
 
-std::optional<std::vector<std::string>> Hdf5FbTf::readTransformStampedFrames(const std::string& id)
+std::optional<std::vector<std::string>> Hdf5FbTf::readTransformStampedFrames(const std::string& id, const bool isStatic)
 {
   const std::scoped_lock lock(*m_write_mtx);
 
-  std::string hdf5GroupPath = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF + "/" + id;
+  std::string hdf5_group_tf;
+  if (isStatic)
+  {
+    hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF_STATIC;
+  }
+  else
+  {
+    hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF;
+  }
+
+  std::string hdf5GroupPath = hdf5_group_tf + "/" + id;
 
   if (!m_file->exist(hdf5GroupPath))
   {

diff --git a/seerep_hdf5/seerep_hdf5_pb/include/seerep_hdf5_pb/hdf5_pb_tf.h b/seerep_hdf5/seerep_hdf5_pb/include/seerep_hdf5_pb/hdf5_pb_tf.h
@@ -23,14 +23,9 @@ class Hdf5PbTf : public Hdf5PbGeneral
 
   void writeTransformStamped(const seerep::pb::TransformStamped& tf);
 
-  std::optional<std::vector<seerep::pb::TransformStamped>> readTransformStamped(const std::string& id);
-  std::optional<std::vector<std::string>> readTransformStampedFrames(const std::string& id);
-
-private:
-  // datatype group names in hdf5
-  inline static const std::string HDF5_GROUP_TF = "tf";
-
-  inline static const std::string SIZE = "size";
+  std::optional<std::vector<seerep::pb::TransformStamped>> readTransformStamped(const std::string& id,
+                                                                                const bool isStatic);
+  std::optional<std::vector<std::string>> readTransformStampedFrames(const std::string& id, const bool isStatic);
 };
 
 } /* namespace seerep_hdf5_pb */