From 07826c4c67e779350d0274ff3e516b806cab3d69 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Mon, 8 May 2023 15:57:27 +0200 Subject: [PATCH 1/7] save seerep version in hdf5 file --- .../seerep_hdf5_core/hdf5_core_general.h | 4 +++ .../src/hdf5_core_general.cpp | 30 +++++++++++++++++++ seerep_msgs/core/project_info.h | 1 + seerep_msgs/fbs/project_info.fbs | 1 + seerep_msgs/protos/project_info.proto | 1 + .../include/seerep_core/core_project.h | 10 ++++++- seerep_srv/seerep_core/src/core.cpp | 3 +- seerep_srv/seerep_core/src/core_project.cpp | 16 ++++++++-- .../seerep_server/src/fb_meta_operations.cpp | 8 +++-- .../seerep_server/src/pb_meta_operations.cpp | 4 +++ 10 files changed, 72 insertions(+), 6 deletions(-) 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 index a6a9637bf..ae4d02bc3 100644 --- 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::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/src/hdf5_core_general.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp index c4d8bba72..ea375a7f5 100644 --- 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,36 @@ 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(VERSION, version); + } + else + { + m_file->getAttribute(VERSION).write(version); + } + m_file->flush(); +} +const std::string Hdf5CoreGeneral::readVersion() +{ + const std::scoped_lock lock(*m_write_mtx); + + std::string version; + try + { + m_file->getAttribute(VERSION).read(version); + } + catch (...) + { + throw std::runtime_error("Project " + m_file->getName() + " has no version defined."); + } + return version; +} + std::optional Hdf5CoreGeneral::readFrameId(const std::string& datatypeGroup, const std::string& uuid) { std::string id = datatypeGroup + "/" + uuid; diff --git a/seerep_msgs/core/project_info.h b/seerep_msgs/core/project_info.h index f8ee914d5..660ad2803 100644 --- a/seerep_msgs/core/project_info.h +++ b/seerep_msgs/core/project_info.h @@ -14,6 +14,7 @@ struct ProjectInfo boost::uuids::uuid uuid; std::string frameId; GeodeticCoordinates geodetCoords; + std::string version; }; } /* namespace seerep_core_msgs */ diff --git a/seerep_msgs/fbs/project_info.fbs b/seerep_msgs/fbs/project_info.fbs index 01cf20f52..95fa7dcd6 100644 --- a/seerep_msgs/fbs/project_info.fbs +++ b/seerep_msgs/fbs/project_info.fbs @@ -8,4 +8,5 @@ table ProjectInfo uuid:string; frameid:string; geodetic_position:seerep.fb.GeodeticCoordinates; + version:string; } diff --git a/seerep_msgs/protos/project_info.proto b/seerep_msgs/protos/project_info.proto index 65a148b31..c3e5a6df8 100644 --- a/seerep_msgs/protos/project_info.proto +++ b/seerep_msgs/protos/project_info.proto @@ -10,4 +10,5 @@ message ProjectInfo string uuid = 2; string frameid = 3; GeodeticCoordinates geodeticCoordinates = 4; + string version = 5; } diff --git a/seerep_srv/seerep_core/include/seerep_core/core_project.h b/seerep_srv/seerep_core/include/seerep_core/core_project.h index 4f916895c..7cf56ac4d 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_project.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_project.h @@ -64,7 +64,8 @@ class CoreProject * @param geodetic coordinates for the location of the site of data recording */ CoreProject(const boost::uuids::uuid& uuid, const std::string path, const std::string projectname, - const std::string mapFrameId, const seerep_core_msgs::GeodeticCoordinates geodeticCoords); + const std::string mapFrameId, const seerep_core_msgs::GeodeticCoordinates geodeticCoords, + const std::string version); ~CoreProject(); /** @@ -77,6 +78,11 @@ class CoreProject * @return the map frame id */ std::string getFrameId(); + /** + * @brief Returns the version of seerep used to create the project + * @return seerep version string + */ + const std::string getVersion(); /** * @brief Returns a vector of UUIDs of datasets that match the query and the project UUID @@ -186,6 +192,8 @@ class CoreProject std::string m_frameId; /** @brief the geodetic coordinates of the location where the data was collected in this project */ std::optional m_geodeticCoordinates; + /** @brief the version of seerep used for this project */ + std::string m_version; /** @brief the write mutex for the HDF5 file of this project */ std::shared_ptr m_write_mtx; diff --git a/seerep_srv/seerep_core/src/core.cpp b/seerep_srv/seerep_core/src/core.cpp index 07998f079..f2c987510 100644 --- a/seerep_srv/seerep_core/src/core.cpp +++ b/seerep_srv/seerep_core/src/core.cpp @@ -110,7 +110,7 @@ void Core::createProject(const seerep_core_msgs::ProjectInfo& projectInfo) std::string path = m_dataFolder + "/" + filename + ".h5"; auto project = std::make_shared(projectInfo.uuid, path, projectInfo.name, projectInfo.frameId, - projectInfo.geodetCoords); + projectInfo.geodetCoords, projectInfo.version); m_projects.insert(std::make_pair(projectInfo.uuid, project)); } @@ -123,6 +123,7 @@ std::vector Core::getProjects() projectinfo.name = it->second->getName(); projectinfo.uuid = it->first; projectinfo.frameId = it->second->getFrameId(); + projectinfo.version = it->second->getVersion(); projectinfo.geodetCoords = it->second->getGeodeticCoordinates(); projectInfos.push_back(projectinfo); diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index b7ff7562b..5d53577c8 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -8,6 +8,7 @@ CoreProject::CoreProject(const boost::uuids::uuid& uuid, const std::string path) m_projectname = m_ioGeneral->readProjectname(); m_frameId = m_ioGeneral->readProjectFrameId(); + m_version = m_ioGeneral->readVersion(); /// TODO use the advantages of std::optional auto geodeticCoordinates = m_ioGeneral->readGeodeticLocation(); @@ -19,13 +20,20 @@ CoreProject::CoreProject(const boost::uuids::uuid& uuid, const std::string path) recreateDatatypes(); } CoreProject::CoreProject(const boost::uuids::uuid& uuid, const std::string path, const std::string projectname, - const std::string mapFrameId, const seerep_core_msgs::GeodeticCoordinates geodeticCoords) - : m_uuid(uuid), m_path(path), m_projectname(projectname), m_frameId(mapFrameId), m_geodeticCoordinates(geodeticCoords) + const std::string mapFrameId, const seerep_core_msgs::GeodeticCoordinates geodeticCoords, + const std::string version) + : m_uuid(uuid) + , m_path(path) + , m_projectname(projectname) + , m_frameId(mapFrameId) + , m_geodeticCoordinates(geodeticCoords) + , m_version(version) { createHdf5Io(m_path); m_ioGeneral->writeProjectname(m_projectname); m_ioGeneral->writeProjectFrameId(m_frameId); m_ioGeneral->writeGeodeticLocation(m_geodeticCoordinates.value()); + m_ioGeneral->writeVersion(m_version); recreateDatatypes(); } CoreProject::~CoreProject() @@ -40,6 +48,10 @@ std::string CoreProject::getFrameId() { return m_frameId; } +const std::string CoreProject::getVersion() +{ + return m_version; +} seerep_core_msgs::QueryResultProject CoreProject::getDataset(const seerep_core_msgs::Query& query) { diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index f59d273b9..ddc8a66bc 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -1,5 +1,7 @@ #include "seerep_server/fb_meta_operations.h" +extern const char* GIT_TAG; + namespace seerep_server { FbMetaOperations::FbMetaOperations(std::shared_ptr seerepCore) : seerepCore(seerepCore) @@ -16,6 +18,7 @@ grpc::Status FbMetaOperations::CreateProject(grpc::ServerContext* context, seerep_core_msgs::ProjectInfo projectInfo; projectInfo.frameId = requestMsg->map_frame_id()->str(); projectInfo.name = requestMsg->name()->str(); + projectInfo.version = GIT_TAG; projectInfo.uuid = boost::uuids::random_generator()(); // extracting geodetic coordinates attribute information from flatbuffer and saving in seerep core msg struct @@ -56,6 +59,7 @@ grpc::Status FbMetaOperations::GetProjects(grpc::ServerContext* context, auto nameOffset = builder.CreateString(projectInfo.name); auto uuidOffset = builder.CreateString(boost::lexical_cast(projectInfo.uuid)); auto frameIdOffset = builder.CreateString(projectInfo.frameId); + auto versionOffset = builder.CreateString(projectInfo.version); auto coordinateSystemOffset = builder.CreateString(projectInfo.geodetCoords.coordinateSystem); auto ellipsoidOffset = builder.CreateString(projectInfo.geodetCoords.ellipsoid); @@ -68,8 +72,8 @@ grpc::Status FbMetaOperations::GetProjects(grpc::ServerContext* context, gcbuilder.add_longitude(projectInfo.geodetCoords.longitude); auto geodeticCoordinatesOffset = gcbuilder.Finish(); - projectInfosVector.push_back( - seerep::fb::CreateProjectInfo(builder, nameOffset, uuidOffset, frameIdOffset, geodeticCoordinatesOffset)); + projectInfosVector.push_back(seerep::fb::CreateProjectInfo(builder, nameOffset, uuidOffset, frameIdOffset, + geodeticCoordinatesOffset, versionOffset)); } auto vectorOffset = builder.CreateVector(projectInfosVector); auto projectInfosOffset = seerep::fb::CreateProjectInfos(builder, vectorOffset); diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 07d9889be..66cc5e58d 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -1,5 +1,7 @@ #include "seerep_server/pb_meta_operations.h" +extern const char* GIT_TAG; + namespace seerep_server { PbMetaOperations::PbMetaOperations(std::shared_ptr seerepCore) : seerepCore(seerepCore) @@ -17,6 +19,7 @@ grpc::Status PbMetaOperations::CreateProject(grpc::ServerContext* context, const seerep_core_msgs::ProjectInfo projectInfo; projectInfo.frameId = request->mapframeid(); projectInfo.name = request->name(); + projectInfo.version = GIT_TAG; projectInfo.uuid = boost::uuids::random_generator()(); // assigning geodetic coords attributes individually @@ -66,6 +69,7 @@ grpc::Status PbMetaOperations::GetProjects(grpc::ServerContext* context, const g responseProjectInfo->set_name(projectInfo.name); responseProjectInfo->set_uuid(boost::lexical_cast(projectInfo.uuid)); responseProjectInfo->set_frameid(projectInfo.frameId); + responseProjectInfo->set_version(projectInfo.version); // assigning geodetic coords attributes individually responseProjectInfo->mutable_geodeticcoordinates()->set_coordinatesystem( From 46cdef9d5f6a476d160df7dca8ca47d973e63001 Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Tue, 16 May 2023 17:09:37 +0200 Subject: [PATCH 2/7] add tf_static --- .../seerep_ros_communication/src/client.cpp | 4 +- .../src/clientImagesWithDetection.cpp | 2 +- .../seerep_ros_communication/src/hdf5dump.cpp | 2 +- .../include/seerep_hdf5_core/hdf5_core_tf.h | 6 +- .../seerep_hdf5_core/src/hdf5_core_tf.cpp | 30 ++++++++-- seerep_hdf5/seerep_hdf5_fb/CMakeLists.txt | 3 + .../cmake/SeerepHdf5FbConfig.cmake.in | 6 +- .../include/seerep_hdf5_fb/hdf5_fb_tf.h | 4 +- seerep_hdf5/seerep_hdf5_fb/package.xml | 1 + seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp | 41 +++++++++++-- .../include/seerep_hdf5_pb/hdf5_pb_tf.h | 11 +--- seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_tf.cpp | 57 ++++++++++++++---- seerep_msgs/fbs/transform_stamped.fbs | 1 + seerep_msgs/protos/transform_stamped.proto | 1 + .../seerep_ros_conversions_fb/conversions.h | 23 ++++---- .../seerep_ros_conversions_fb/conversions.cpp | 59 +++++++++++++------ .../seerep_ros_conversions_pb/conversions.h | 3 +- .../seerep_ros_conversions_pb/conversions.cpp | 4 +- .../seerep_core/include/seerep_core/core_tf.h | 6 +- seerep_srv/seerep_core/src/core_tf.cpp | 21 +++++-- seerep_srv/seerep_core_fb/src/core_fb_tf.cpp | 2 +- seerep_srv/seerep_core_pb/src/core_pb_tf.cpp | 2 +- 22 files changed, 209 insertions(+), 80 deletions(-) diff --git a/examples/cpp/seerep_ros_communication/src/client.cpp b/examples/cpp/seerep_ros_communication/src/client.cpp index adaec8402..31057f9f1 100644 --- 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 index 389d00705..8ca2794d8 100644 --- 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 index 32c16db55..05530f29d 100644 --- 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_tf.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_tf.h index 357c16269..ad4c53954 100644 --- 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& file, std::shared_ptr& write_mtx); - std::optional> readTransformStamped(const std::string& id); - std::optional> readTransformStampedFrames(const std::string& id); + std::optional> readTransformStamped(const std::string& id, + const bool isStatic); + std::optional> readTransformStampedFrames(const std::string& id, const bool isStatic); private: std::string readFrame(const std::string& frameName, const std::shared_ptr& 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_tf.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_tf.cpp index 386125c46..734e56b7a 100644 --- 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& file, std::shared_ptr> Hdf5CoreTf::readTransformStamped(const std::string& id) +std::optional> 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> Hdf5CoreTf::readTran return convertToTfs(size, parentframe, childframe, time, trans, rot); } -std::optional> Hdf5CoreTf::readTransformStampedFrames(const std::string& id) +std::optional> 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 index 0138a265f..b935ad8fd 100644 --- 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 index 1f5e64b75..e146076b3 100644 --- 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 index 725897ffe..86262f894 100644 --- 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>> - readTransformStamped(const std::string& id); - std::optional> readTransformStampedFrames(const std::string& id); + readTransformStamped(const std::string& id, const bool isStatic); + std::optional> 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 index cefff7417..1c283edfc 100644 --- a/seerep_hdf5/seerep_hdf5_fb/package.xml +++ b/seerep_hdf5/seerep_hdf5_fb/package.xml @@ -17,6 +17,7 @@ seerep_msgs seerep_com seerep_hdf5_core + geometry_msgs cmake diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp index 138a068b4..cd834a480 100644 --- 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>> -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> Hdf5FbTf::readTransformStampedFrames(const std::string& id) +std::optional> 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 index 6cfa35385..d7639c0fa 100644 --- 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> readTransformStamped(const std::string& id); - std::optional> 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> readTransformStamped(const std::string& id, + const bool isStatic); + std::optional> readTransformStampedFrames(const std::string& id, const bool isStatic); }; } /* namespace seerep_hdf5_pb */ diff --git a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_tf.cpp b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_tf.cpp index b9008d810..97d5a2823 100644 --- a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_tf.cpp +++ b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_tf.cpp @@ -13,7 +13,19 @@ Hdf5PbTf::Hdf5PbTf(std::shared_ptr& file, std::shared_ptr(m_file->getDataSet(hdf5DatasetRotPath)); HighFive::Group group = m_file->getGroup(hdf5DatasetPath); - group.getAttribute(SIZE).read(size); + group.getAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE).read(size); // Resize the dataset to a larger size data_set_time_ptr->resize({ size + 1, 2 }); @@ -94,21 +106,34 @@ void Hdf5PbTf::writeTransformStamped(const seerep::pb::TransformStamped& tf) // write the size as group attribute HighFive::Group group = m_file->getGroup(hdf5DatasetPath); - if (!group.hasAttribute(SIZE)) + if (!group.hasAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE)) { - group.createAttribute(SIZE, ++size); + group.createAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE, ++size); } else { - group.getAttribute(SIZE).write(++size); + group.getAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE).write(++size); } m_file->flush(); } -std::optional> Hdf5PbTf::readTransformStamped(const std::string& id) +std::optional> Hdf5PbTf::readTransformStamped(const std::string& id, + const bool isStatic) { - std::string hdf5GroupPath = HDF5_GROUP_TF + "/" + id; + const std::scoped_lock lock(*m_write_mtx); + + 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"; @@ -124,7 +149,7 @@ std::optional> Hdf5PbTf::readTransform // read size std::shared_ptr group_ptr = std::make_shared(m_file->getGroup(hdf5GroupPath)); long unsigned int size; - group_ptr->getAttribute(SIZE).read(size); + group_ptr->getAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE).read(size); if (size == 0) { std::cout << "tf data has size 0." << std::endl; @@ -191,9 +216,21 @@ std::optional> Hdf5PbTf::readTransform return tfs; } -std::optional> Hdf5PbTf::readTransformStampedFrames(const std::string& id) +std::optional> Hdf5PbTf::readTransformStampedFrames(const std::string& id, const bool isStatic) { - std::string hdf5GroupPath = HDF5_GROUP_TF + "/" + id; + const std::scoped_lock lock(*m_write_mtx); + + 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_msgs/fbs/transform_stamped.fbs b/seerep_msgs/fbs/transform_stamped.fbs index ae5d14fdf..6530065f9 100644 --- a/seerep_msgs/fbs/transform_stamped.fbs +++ b/seerep_msgs/fbs/transform_stamped.fbs @@ -8,4 +8,5 @@ table TransformStamped { header:seerep.fb.Header; child_frame_id:string; transform:seerep.fb.Transform; + is_static:bool; } diff --git a/seerep_msgs/protos/transform_stamped.proto b/seerep_msgs/protos/transform_stamped.proto index 3d1769e94..0387be9ad 100644 --- a/seerep_msgs/protos/transform_stamped.proto +++ b/seerep_msgs/protos/transform_stamped.proto @@ -10,4 +10,5 @@ message TransformStamped Header header = 1; string child_frame_id = 2; Transform transform = 3; + bool is_static = 4; } diff --git a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h index bffa7737e..9607da77f 100644 --- a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h +++ b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h @@ -124,7 +124,7 @@ sensor_msgs::PointCloud2 toROS(const seerep::fb::PointCloud2& cloud); * @return gRPC Flatbuffer Image message */ flatbuffers::grpc::Message toFlat(const sensor_msgs::Image& image, std::string projectuuid, - std::string msguuid); + std::string msguuid = ""); /** * @brief Converts a ROS sensor_msgs/Image message to the corresponding * Flatbuffer Image message @@ -135,7 +135,7 @@ flatbuffers::grpc::Message toFlat(const sensor_msgs::Image& i * @return Flatbuffer Image message */ flatbuffers::Offset toFlat(const sensor_msgs::Image& image, std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder, std::string msguuid); + flatbuffers::grpc::MessageBuilder& builder, std::string msguuid = ""); /** * @brief Converts a Flatbuffer Image message to the corresponding @@ -335,7 +335,7 @@ geometry_msgs::Transform toROS(const seerep::fb::Transform& transform); * @return gRPC Flatbuffer TransformStamped message */ flatbuffers::grpc::Message toFlat(const geometry_msgs::TransformStamped& transform, - std::string projectuuid); + std::string projectuuid, const bool isStatic); /** * @brief Converts a ROS geometry_msgs::TransformStamped message to the corresponding * Flatbuffer TransformStamped message @@ -345,7 +345,7 @@ flatbuffers::grpc::Message toFlat(const geometry_m * @return Flatbuffer TransformStamped message */ flatbuffers::Offset toFlat(const geometry_msgs::TransformStamped& transform, - std::string projectuuid, + std::string projectuuid, const bool isStatic, flatbuffers::grpc::MessageBuilder& builder); /** @@ -366,7 +366,8 @@ geometry_msgs::TransformStamped toROS(const seerep::fb::TransformStamped& transf */ flatbuffers::grpc::Message toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, std::string category, - std::string msguuid); + std::string msguuid = "", std::vector labels = std::vector(), + std::vector instances = std::vector()); /** * @brief Converts a ROS vision_msgs::Detection2DArray message to the corresponding * Flatbuffer BoundingBoxes2D_labeled_stamped message @@ -376,10 +377,11 @@ toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid * @param msguuid UUID of the message. Used to set the corresponding field in the Flatbuffer message * @return Flatbuffer BoundingBoxes2D_labeled_stamped message */ -flatbuffers::Offset toFlat(const vision_msgs::Detection2DArray& detection2d, - std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder, - std::string category, std::string msguuid); +flatbuffers::Offset +toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, + flatbuffers::grpc::MessageBuilder& builder, std::string category, std::string msguuid = "", + std::vector labels = std::vector(), + std::vector instances = std::vector()); /** * @brief Converts a Flatbuffer BoundingBoxes2D_labeled_stamped message to the corresponding @@ -404,7 +406,8 @@ flatbuffers::grpc::Message toFlat(const vision * @return Flatbuffer BoundingBoxes2DLabeled message */ flatbuffers::Offset toFlat(const vision_msgs::Detection2D& detection2d, - flatbuffers::grpc::MessageBuilder& builder); + flatbuffers::grpc::MessageBuilder& builder, + std::string label = "", std::string instanceUUID = ""); /** * @brief Converts a Flatbuffer BoundingBoxes2DLabeled message to the corresponding diff --git a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp index 32e48a6f5..46be96dde 100644 --- a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp +++ b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp @@ -139,14 +139,14 @@ sensor_msgs::PointCloud2 toROS(const seerep::fb::PointCloud2& cloud) * Image */ flatbuffers::grpc::Message toFlat(const sensor_msgs::Image& image, std::string projectuuid, - std::string msguuid = "") + std::string msguuid) { flatbuffers::grpc::MessageBuilder builder; builder.Finish(toFlat(image, projectuuid, builder, msguuid)); return builder.ReleaseMessage(); } flatbuffers::Offset toFlat(const sensor_msgs::Image& image, std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder, std::string msguuid = "") + flatbuffers::grpc::MessageBuilder& builder, std::string msguuid) { auto header = toFlat(image.header, projectuuid, builder, msguuid); @@ -387,14 +387,14 @@ geometry_msgs::Transform toROS(const seerep::fb::Transform& transform) * TransformStamped */ flatbuffers::grpc::Message toFlat(const geometry_msgs::TransformStamped& transform, - std::string projectuuid) + std::string projectuuid, const bool isStatic) { flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(transform, projectuuid, builder)); + builder.Finish(toFlat(transform, projectuuid, isStatic, builder)); return builder.ReleaseMessage(); } flatbuffers::Offset toFlat(const geometry_msgs::TransformStamped& transform, - std::string projectuuid, + std::string projectuuid, const bool isStatic, flatbuffers::grpc::MessageBuilder& builder) { auto headerOffset = toFlat(transform.header, projectuuid, builder); @@ -405,6 +405,7 @@ flatbuffers::Offset toFlat(const geometry_msgs::Tr transformbuilder.add_header(headerOffset); transformbuilder.add_child_frame_id(frameIdOffset); transformbuilder.add_transform(transformOffset); + transformbuilder.add_is_static(isStatic); return transformbuilder.Finish(); } @@ -423,16 +424,16 @@ geometry_msgs::TransformStamped toROS(const seerep::fb::TransformStamped& transf flatbuffers::grpc::Message toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, std::string category, - std::string msguuid = "") + std::string msguuid, std::vector labels, std::vector instances) { flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(detection2d, projectuuid, builder, category, msguuid)); + builder.Finish(toFlat(detection2d, projectuuid, builder, category, msguuid, labels, instances)); return builder.ReleaseMessage(); } -flatbuffers::Offset toFlat(const vision_msgs::Detection2DArray& detection2d, - std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder, - std::string category, std::string msguuid = "") +flatbuffers::Offset +toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, + flatbuffers::grpc::MessageBuilder& builder, std::string category, std::string msguuid, + std::vector labels, std::vector instances) { // convert header auto header = toFlat(detection2d.header, projectuuid, builder, msguuid); @@ -440,10 +441,20 @@ flatbuffers::Offset toFlat(const visi // create boundingbox labeled vector std::vector> bblabeled; - // for each loop for saving in fb bb_labeled vector - for (vision_msgs::Detection2D detection : detection2d.detections) + if (detection2d.detections.size() == labels.size() && detection2d.detections.size() == instances.size()) { - bblabeled.push_back(toFlat(detection, builder)); + for (long unsigned int i = 0; i < bblabeled.size(); i++) + { + bblabeled.push_back(toFlat(detection2d.detections.at(i), builder)); + } + } + else + { + // for each loop for saving in fb bb_labeled vector + for (vision_msgs::Detection2D detection : detection2d.detections) + { + bblabeled.push_back(toFlat(detection, builder)); + } } // fb labels vector @@ -477,7 +488,8 @@ flatbuffers::grpc::Message toFlat(const vision return builder.ReleaseMessage(); } flatbuffers::Offset toFlat(const vision_msgs::Detection2D& detection2d, - flatbuffers::grpc::MessageBuilder& builder) + flatbuffers::grpc::MessageBuilder& builder, + std::string label, std::string instanceUUID) { seerep::fb::Point2DBuilder pointBuilderCenter(builder); pointBuilderCenter.add_x(detection2d.bbox.center.x); @@ -494,17 +506,26 @@ flatbuffers::Offset toFlat(const vision_msgs:: bbbuilder.add_spatial_extent(pointSpatialExtent); auto bb = bbbuilder.Finish(); - auto InstanceOffset = builder.CreateString(""); - auto labelOffset = builder.CreateString(std::to_string(detection2d.results.at(0).id)); + flatbuffers::Offset labelOffset; + if (label == "") + { + labelOffset = builder.CreateString(std::to_string(detection2d.results.at(0).id)); + } + else + { + labelOffset = builder.CreateString(label); + } + + flatbuffers::Offset InstanceOffset = builder.CreateString(instanceUUID); seerep::fb::LabelBuilder labelBuilder(builder); labelBuilder.add_label(labelOffset); labelBuilder.add_confidence(detection2d.results.at(0).score); - auto label = labelBuilder.Finish(); + auto labelMsg = labelBuilder.Finish(); seerep::fb::LabelWithInstanceBuilder labelInstanceBuilder(builder); labelInstanceBuilder.add_instanceUuid(InstanceOffset); - labelInstanceBuilder.add_label(label); + labelInstanceBuilder.add_label(labelMsg); auto labelWithInstanceOffset = labelInstanceBuilder.Finish(); seerep::fb::BoundingBox2DLabeledBuilder bblabeledbuilder(builder); diff --git a/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h b/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h index e0fec2f1b..51786dde4 100644 --- a/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h +++ b/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h @@ -208,7 +208,8 @@ geometry_msgs::Transform toROS(const seerep::pb::Transform& transform); * @param optional projectuuid std::string * @return Protobuf TransformStamped message */ -seerep::pb::TransformStamped toProto(const geometry_msgs::TransformStamped& transform, std::string projectuuid = ""); +seerep::pb::TransformStamped toProto(const geometry_msgs::TransformStamped& transform, const bool isStatic, + std::string projectuuid = ""); /** * @brief Converts a Protobuf TransformStamped message to the corresponding diff --git a/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp b/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp index 3100012e2..0e258ed42 100644 --- a/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp +++ b/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp @@ -262,7 +262,8 @@ geometry_msgs::Transform toROS(const seerep::pb::Transform& transform) /* * TransformStamped */ -seerep::pb::TransformStamped toProto(const geometry_msgs::TransformStamped& transform, std::string projectuuid) +seerep::pb::TransformStamped toProto(const geometry_msgs::TransformStamped& transform, const bool isStatic, + std::string projectuuid) { seerep::pb::TransformStamped ret; *ret.mutable_header() = toProto(transform.header); @@ -270,6 +271,7 @@ seerep::pb::TransformStamped toProto(const geometry_msgs::TransformStamped& tran *ret.mutable_child_frame_id() = transform.child_frame_id; *ret.mutable_header()->mutable_uuid_project() = projectuuid; *ret.mutable_transform() = toProto(transform.transform); + ret.set_is_static(isStatic); return ret; } diff --git a/seerep_srv/seerep_core/include/seerep_core/core_tf.h b/seerep_srv/seerep_core/include/seerep_core/core_tf.h index 8f3dfea84..f90e20fc6 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_tf.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_tf.h @@ -59,7 +59,7 @@ class CoreTf * @brief Adds a tf to the tf buffer * @param tf the TransformStamped to be added to the buffer */ - void addDataset(const geometry_msgs::TransformStamped& tf); + void addDataset(const geometry_msgs::TransformStamped& tf, const bool isStatic = false); /** * @brief Transforms an AABB into the target frame @@ -103,11 +103,13 @@ class CoreTf * @brief loads the TFs into the buffer from the HDF5 file */ void recreateDatasets(); + + void loadTfs(const std::vector tfs, const bool isStatic); /** * @brief adds a transformation to the TF buffer * @param transform the transformation to be added to the buffer */ - void addToTfBuffer(geometry_msgs::TransformStamped transform); + void addToTfBuffer(geometry_msgs::TransformStamped transform, const bool isStatic); /** * @brief Transforms the AABB based on the min/max coordinates into another frame. Transforms all 8 vertices of the diff --git a/seerep_srv/seerep_core/src/core_tf.cpp b/seerep_srv/seerep_core/src/core_tf.cpp index de0477c53..581eb32e9 100644 --- a/seerep_srv/seerep_core/src/core_tf.cpp +++ b/seerep_srv/seerep_core/src/core_tf.cpp @@ -15,18 +15,27 @@ CoreTf::~CoreTf() void CoreTf::recreateDatasets() { std::vector tfs = m_hdf5_io->getGroupDatasets(seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF); + loadTfs(tfs, false); + + std::vector tfs_static = m_hdf5_io->getGroupDatasets(seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF_STATIC); + loadTfs(tfs_static, true); +} + +void CoreTf::loadTfs(std::vector tfs, const bool isStatic) +{ for (auto const& name : tfs) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "found " << name << " in HDF5 file."; try { - std::optional> transforms = m_hdf5_io->readTransformStamped(name); + std::optional> transforms = + m_hdf5_io->readTransformStamped(name, isStatic); if (transforms) { for (auto& transform : transforms.value()) { - addToTfBuffer(transform); + addToTfBuffer(transform, isStatic); } } } @@ -52,9 +61,9 @@ std::optional CoreTf::getData(const int64_t& ti } } -void CoreTf::addDataset(const geometry_msgs::TransformStamped& transform) +void CoreTf::addDataset(const geometry_msgs::TransformStamped& transform, const bool isStatic) { - addToTfBuffer(transform); + addToTfBuffer(transform, isStatic); } // TODO optimise! @@ -117,9 +126,9 @@ std::vector CoreTf::getFrames() return std::vector{ m_tfBuffer.allFramesAsYAML() }; } -void CoreTf::addToTfBuffer(geometry_msgs::TransformStamped transform) +void CoreTf::addToTfBuffer(geometry_msgs::TransformStamped transform, const bool isStatic) { - m_tfBuffer.setTransform(transform, "fromHDF5"); + m_tfBuffer.setTransform(transform, "fromHDF5", isStatic); } void CoreTf::getAABBinNewFrame(const tf2::Transform& transform, const std::vector& x, diff --git a/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp b/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp index 6e982bdfc..bc00f830a 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp @@ -22,7 +22,7 @@ void CoreFbTf::getData(const seerep::fb::TransformStampedQuery& query, if (result) { - *response = seerep_ros_conversions_fb::toFlat(result.value(), query.header()->uuid_project()->str()); + *response = seerep_ros_conversions_fb::toFlat(result.value(), query.header()->uuid_project()->str(), false); } else { diff --git a/seerep_srv/seerep_core_pb/src/core_pb_tf.cpp b/seerep_srv/seerep_core_pb/src/core_pb_tf.cpp index 8eb065e1e..12ad2878d 100644 --- a/seerep_srv/seerep_core_pb/src/core_pb_tf.cpp +++ b/seerep_srv/seerep_core_pb/src/core_pb_tf.cpp @@ -24,7 +24,7 @@ std::optional CorePbTf::getData(const seerep::pb:: if (result) { - return seerep_ros_conversions_pb::toProto(result.value()); + return seerep_ros_conversions_pb::toProto(result.value(), false); } else { From 1bb28819703045164b524dd4b525d70eeee1b0fc Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Tue, 16 May 2023 17:22:08 +0200 Subject: [PATCH 3/7] fix ros conversion tests --- .../test/ros_to_fb_conversion_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/seerep_ros/seerep_ros_conversions_fb/test/ros_to_fb_conversion_test.cpp b/seerep_ros/seerep_ros_conversions_fb/test/ros_to_fb_conversion_test.cpp index 8ffa162f7..727b18039 100644 --- a/seerep_ros/seerep_ros_conversions_fb/test/ros_to_fb_conversion_test.cpp +++ b/seerep_ros/seerep_ros_conversions_fb/test/ros_to_fb_conversion_test.cpp @@ -426,7 +426,7 @@ class rosToFbConversionTest : public testing::Test original_t_stamped = createTransformStamped(); flatbuffers::grpc::Message fb_transform_stamped; - fb_transform_stamped = seerep_ros_conversions_fb::toFlat(original_t_stamped, p_uuid); + fb_transform_stamped = seerep_ros_conversions_fb::toFlat(original_t_stamped, p_uuid, false); converted_t_stamped = seerep_ros_conversions_fb::toROS(*fb_transform_stamped.GetRoot()); // Transform End From 2149f8f392a9640aea76e98291bab751d89d1edf Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Tue, 16 May 2023 18:53:46 +0200 Subject: [PATCH 4/7] fix start_server.sh --- docker/start_server.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/start_server.sh b/docker/start_server.sh index d97fa545e..cd2abd563 100755 --- 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 "$@" From 27c7ad6275c6d9f5c5b99dd0800dd18200a422a7 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 17 May 2023 13:10:04 +0200 Subject: [PATCH 5/7] update test action strucutre additionally enable tests to run for each commit --- .github/workflows/test.yml | 52 ++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 7f42af9e3..35d95e315 100644 --- 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 From 7d0c4a735715dc001559417e592eac26291952f1 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 19 May 2023 17:26:34 +0200 Subject: [PATCH 6/7] fix: vs code server startup config --- .devcontainer/.vscode/launch.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/.vscode/launch.json b/.devcontainer/.vscode/launch.json index 95c285322..f5e280ccc 100644 --- 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", From 3f74243813fd6a0cdbbe96e5f1301f51c087a482 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Mon, 22 May 2023 13:13:08 +0200 Subject: [PATCH 7/7] make version number optional to ensure compatibility with legacy files --- .../include/seerep_hdf5_core/hdf5_core_general.h | 2 +- .../seerep_hdf5_core/src/hdf5_core_general.cpp | 11 +++++++---- seerep_srv/seerep_core/src/core_project.cpp | 12 +++++++++--- 3 files changed, 17 insertions(+), 8 deletions(-) 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 index ae4d02bc3..7308850f7 100644 --- 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 @@ -161,7 +161,7 @@ class Hdf5CoreGeneral std::string readProjectFrameId(); void writeVersion(const std::string& version); - const std::string readVersion(); + const std::optional readVersion(); // ################ // Geodetic Coordinates diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp index ea375a7f5..811e1d29d 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp @@ -109,19 +109,22 @@ void Hdf5CoreGeneral::writeVersion(const std::string& version) } m_file->flush(); } -const std::string Hdf5CoreGeneral::readVersion() + +const std::optional Hdf5CoreGeneral::readVersion() { const std::scoped_lock lock(*m_write_mtx); std::string version; try { - m_file->getAttribute(VERSION).read(version); + version = readAttributeFromHdf5(m_file->getName(), *m_file, VERSION); } - catch (...) + catch (const std::exception& e) { - throw std::runtime_error("Project " + m_file->getName() + " has no version defined."); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << e.what(); + return std::nullopt; } + return version; } diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index 5d53577c8..bdc82e810 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -8,17 +8,23 @@ CoreProject::CoreProject(const boost::uuids::uuid& uuid, const std::string path) m_projectname = m_ioGeneral->readProjectname(); m_frameId = m_ioGeneral->readProjectFrameId(); - m_version = m_ioGeneral->readVersion(); - /// TODO use the advantages of std::optional + + /* get optional class members */ auto geodeticCoordinates = m_ioGeneral->readGeodeticLocation(); + auto version = m_ioGeneral->readVersion(); + + if (version) + { + m_version = version.value(); + } if (geodeticCoordinates) { m_geodeticCoordinates = geodeticCoordinates.value(); } - recreateDatatypes(); } + CoreProject::CoreProject(const boost::uuids::uuid& uuid, const std::string path, const std::string projectname, const std::string mapFrameId, const seerep_core_msgs::GeodeticCoordinates geodeticCoords, const std::string version)