From c8cf95e1906746699a1f1a4d069108bcb2a0c4e2 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Fri, 9 Dec 2022 18:29:26 +0000 Subject: [PATCH 01/21] finished but untested --- seerep_com/fbs/meta_operations.fbs | 4 ++ .../seerep_core/include/seerep_core/core.h | 3 + .../include/seerep_core/core_dataset.h | 8 +++ .../include/seerep_core/core_project.h | 3 + seerep_srv/seerep_core/src/core.cpp | 12 ++++ seerep_srv/seerep_core/src/core_dataset.cpp | 10 ++++ seerep_srv/seerep_core/src/core_project.cpp | 10 ++++ .../seerep_server/fb_meta_operations.h | 6 ++ .../seerep_server/src/fb_meta_operations.cpp | 58 +++++++++++++++++++ 9 files changed, 114 insertions(+) diff --git a/seerep_com/fbs/meta_operations.fbs b/seerep_com/fbs/meta_operations.fbs index dc308c078..d773d6372 100644 --- a/seerep_com/fbs/meta_operations.fbs +++ b/seerep_com/fbs/meta_operations.fbs @@ -3,6 +3,9 @@ include "project_info.fbs"; include "project_infos.fbs"; include "projectCreation.fbs"; include "empty.fbs"; +include "boundingbox.fbs"; +include "datatype.fbs"; +include "time_interval.fbs"; namespace seerep.fb; @@ -11,4 +14,5 @@ rpc_service MetaOperations { GetProjects(seerep.fb.Empty):seerep.fb.ProjectInfos; LoadProjects(seerep.fb.Empty):seerep.fb.Empty; DeleteProject(seerep.fb.ProjectInfo):seerep.fb.Empty; + GetOverallTimeInterval(seerep.fb.ProjectInfo):seerep.fb.TimeInterval; } diff --git a/seerep_srv/seerep_core/include/seerep_core/core.h b/seerep_srv/seerep_core/include/seerep_core/core.h index ff3adfed4..8ac95f4e8 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core.h +++ b/seerep_srv/seerep_core/include/seerep_core/core.h @@ -131,6 +131,9 @@ class Core */ void deleteProject(boost::uuids::uuid uuid); + seerep_core_msgs::AabbTime getOverallTimeInterval(boost::uuids::uuid uuid); + seerep_core_msgs::AABB getOverallBound(boost::uuids::uuid uuid); + private: /** * @brief Returns an iterator to the project with the given uuid. Throws an error if not found diff --git a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h index 3104a3c3e..a9092263e 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -119,6 +119,14 @@ class CoreDataset labelWithInstancePerCategory, const boost::uuids::uuid& msgUuid); + /** + * @brief Get the Time Bounds object + * + * @return seerep_core_msgs::AABB + */ + seerep_core_msgs::AabbTime getTimeBounds(); + seerep_core_msgs::AABB getSpatialBounds(); + private: /** * @brief fills the member variables based on the HDF5 file 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 dc23743b7..6ac7afd10 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_project.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_project.h @@ -142,6 +142,9 @@ class CoreProject */ std::shared_ptr getHdf5File(); + seerep_core_msgs::AabbTime getTimeBounds(); + seerep_core_msgs::AABB getSpatialBounds(); + private: /** * @brief Create the HDF5 file accessor and the mutex. Based on that create the diff --git a/seerep_srv/seerep_core/src/core.cpp b/seerep_srv/seerep_core/src/core.cpp index bfae88dd9..2e3c4e162 100644 --- a/seerep_srv/seerep_core/src/core.cpp +++ b/seerep_srv/seerep_core/src/core.cpp @@ -274,4 +274,16 @@ void Core::addDatasetToResult(seerep_core_msgs::QueryResultProject& dataset, see } } +seerep_core_msgs::AabbTime Core::getOverallTimeInterval(boost::uuids::uuid uuid) +{ + auto project = findProject(uuid); + return project->second->getTimeBounds(); +} + +seerep_core_msgs::AABB Core::getOverallBound(boost::uuids::uuid uuid) +{ + auto project = findProject(uuid); + return project->second->getSpatialBounds(); +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index ebf204fa9..7d8f46941 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -517,4 +517,14 @@ void CoreDataset::addLabels(const seerep_core_msgs::Datatype& datatype, datatypeSpecifics->datasetInstancesMap.emplace(msgUuid, instanceUuids); } +seerep_core_msgs::AabbTime CoreDataset::getTimeBounds() +{ + return m_datatypeDatatypeSpecificsMap.at(seerep_core_msgs::Datatype::Point)->timetree.bounds(); +} + +seerep_core_msgs::AABB CoreDataset::getSpatialBounds() +{ + return m_datatypeDatatypeSpecificsMap.at(seerep_core_msgs::Datatype::PointCloud)->rt.bounds(); +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index 130eb18d7..b4a42f713 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -137,4 +137,14 @@ void CoreProject::recreateDatatypes() } } +seerep_core_msgs::AabbTime CoreProject::getTimeBounds() +{ + return m_coreDatasets->getTimeBounds(); +} + +seerep_core_msgs::AABB CoreProject::getSpatialBounds() +{ + return m_coreDatasets->getSpatialBounds(); +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 531e1940c..97db4266c 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -25,6 +25,12 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service grpc::Status DeleteProject(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; + flatbuffers::Offset + GetOverallTimeInterval(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request); + flatbuffers::Offset + GetOverallBoundingBox(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request); private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index f59d273b9..d97028895 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -165,4 +165,62 @@ grpc::Status FbMetaOperations::DeleteProject(grpc::ServerContext* context, return grpc::Status::OK; } +flatbuffers::Offset +FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request) +{ + std::string uuid = request->GetRoot()->uuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString); + + flatbuffers::grpc::MessageBuilder builder; + + seerep::fb::TimestampBuilder minTimeStampBuilder(builder); + minTimeStampBuilder.add_seconds(timeinterval.min_corner().get<0>()); + flatbuffers::Offset min = minTimeStampBuilder.Finish(); + + seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); + maxTimeStampBuilder.add_seconds(timeinterval.max_corner().get<0>()); + flatbuffers::Offset max = maxTimeStampBuilder.Finish(); + + seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); + timeIntervalBuilder.add_time_min(min); + timeIntervalBuilder.add_time_max(max); + flatbuffers::Offset bb = timeIntervalBuilder.Finish(); + + return bb; +} + +flatbuffers::Offset +FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request) +{ + std::string uuid = request->GetRoot()->uuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString); + + flatbuffers::grpc::MessageBuilder builder; + + seerep::fb::PointBuilder minPointBuilder(builder); + minPointBuilder.add_x(overallBB.min_corner().get<0>()); + minPointBuilder.add_y(overallBB.min_corner().get<1>()); + minPointBuilder.add_z(overallBB.min_corner().get<2>()); + flatbuffers::Offset minPoint = minPointBuilder.Finish(); + + seerep::fb::PointBuilder maxPointBuilder(builder); + maxPointBuilder.add_x(overallBB.max_corner().get<0>()); + maxPointBuilder.add_y(overallBB.max_corner().get<1>()); + maxPointBuilder.add_z(overallBB.max_corner().get<2>()); + flatbuffers::Offset maxPoint = maxPointBuilder.Finish(); + + seerep::fb::BoundingboxBuilder boundingBoxBuilder(builder); + boundingBoxBuilder.add_point_min(minPoint); + boundingBoxBuilder.add_point_max(maxPoint); + flatbuffers::Offset bb = boundingBoxBuilder.Finish(); + + return bb; +} + } /* namespace seerep_server */ From 23e85bd761f0ab1d37bb0b9d087c24a9662fdf9c Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Wed, 14 Dec 2022 13:21:22 +0000 Subject: [PATCH 02/21] Add meta script to query overall time --- .../gRPC/meta/gRPC_fb_getOverallTime.py | 42 +++++++++++++++++++ examples/python/gRPC/util_fb.py | 10 +++++ 2 files changed, 52 insertions(+) create mode 100755 examples/python/gRPC/meta/gRPC_fb_getOverallTime.py diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py new file mode 100755 index 000000000..763bb1585 --- /dev/null +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 + +import os +import sys + +import flatbuffers +from fb import TimeInterval +from fb import meta_operations_grpc_fb as metaOperations + +# importing util functions. Assuming that these files are in the parent dir +# examples/python/gRPC/util.py +# examples/python/gRPC/util_fb.py +script_dir = os.path.dirname(__file__) +util_dir = os.path.join(script_dir, '..') +sys.path.append(util_dir) +import util +import util_fb + +builder = flatbuffers.Builder(1024) +# Default server is localhost ! +channel = util.get_gRPC_channel() + + +# 1. Get all projects from the server +projectuuid = util_fb.getProject(builder, channel, 'testproject') + +# 2. Check if the defined project exist; if not exit +if not projectuuid: + exit() + +# 3. Get gRPC service object +stub = metaOperations.MetaOperationsStub(channel) + +projectInfo = util_fb.createProjectInfo(builder, "name", projectuuid) +builder.Finish(projectInfo) +buf = builder.Output() + +responseBuf = stub.GetOverallTimeInterval(bytes(buf)) +response = TimeInterval.TimeInterval.GetRootAs(responseBuf) + +print("time min (sec/nanos): " + str(response.TimeMin().Seconds()) + " / " + str(response.TimeMin().Nanos())) +print("time max (sec/nanos): " + str(response.TimeMax().Seconds()) + " / " + str(response.TimeMax().Nanos())) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 8393c573c..03fb181dc 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -446,3 +446,13 @@ def createTransformStampedQuery(builder, header, childFrameId): TransformStampedQuery.AddHeader(builder, header) TransformStampedQuery.AddChildFrameId(builder, childFrameId) return TransformStampedQuery.End(builder) + + +def createProjectInfo(builder, name, uuid): + nameStr = builder.CreateString(name) + uuidStr = builder.CreateString(uuid) + + ProjectInfo.Start(builder) + ProjectInfo.AddName(builder, nameStr) + ProjectInfo.AddUuid(builder, uuidStr) + return ProjectInfo.End(builder) From 2b15eaa284d6666eb596c0be372f2eb6899aa70e Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 14 Dec 2022 16:04:13 +0000 Subject: [PATCH 03/21] function override fix --- .../seerep_server/fb_meta_operations.h | 12 +++++----- .../seerep_server/src/fb_meta_operations.cpp | 22 ++++++++++++------- 2 files changed, 20 insertions(+), 14 deletions(-) diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 97db4266c..32fd3701f 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -25,12 +25,12 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service grpc::Status DeleteProject(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; - flatbuffers::Offset - GetOverallTimeInterval(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request); - flatbuffers::Offset - GetOverallBoundingBox(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request); + grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) override; + grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) override; private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index d97028895..d91900c42 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -165,9 +165,9 @@ grpc::Status FbMetaOperations::DeleteProject(grpc::ServerContext* context, return grpc::Status::OK; } -flatbuffers::Offset -FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request) +grpc::Status FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) { std::string uuid = request->GetRoot()->uuid()->str(); boost::uuids::string_generator gen; @@ -189,12 +189,15 @@ FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, timeIntervalBuilder.add_time_max(max); flatbuffers::Offset bb = timeIntervalBuilder.Finish(); - return bb; + builder.Finish(bb); + *response = builder.ReleaseMessage(); + + return grpc::Status::OK; } -flatbuffers::Offset -FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request) +grpc::Status FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) { std::string uuid = request->GetRoot()->uuid()->str(); boost::uuids::string_generator gen; @@ -220,7 +223,10 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, boundingBoxBuilder.add_point_max(maxPoint); flatbuffers::Offset bb = boundingBoxBuilder.Finish(); - return bb; + builder.Finish(bb); + *response = builder.ReleaseMessage(); + + return grpc::Status::OK; } } /* namespace seerep_server */ From c1a96e506cd5106574ed357fac38c5af54bf4100 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 14 Dec 2022 16:05:03 +0000 Subject: [PATCH 04/21] overall bb function definition --- seerep_com/fbs/meta_operations.fbs | 1 + 1 file changed, 1 insertion(+) diff --git a/seerep_com/fbs/meta_operations.fbs b/seerep_com/fbs/meta_operations.fbs index d773d6372..508851abb 100644 --- a/seerep_com/fbs/meta_operations.fbs +++ b/seerep_com/fbs/meta_operations.fbs @@ -15,4 +15,5 @@ rpc_service MetaOperations { LoadProjects(seerep.fb.Empty):seerep.fb.Empty; DeleteProject(seerep.fb.ProjectInfo):seerep.fb.Empty; GetOverallTimeInterval(seerep.fb.ProjectInfo):seerep.fb.TimeInterval; + GetOverallBoundingBox(seerep.fb.ProjectInfo):seerep.fb.Boundingbox; } From 1584d04cfe7f1baac680d586ff93706a70e2a72c Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 14 Dec 2022 16:06:10 +0000 Subject: [PATCH 05/21] running example with apparent logical error --- .../meta/gRPC_fb_getOverallBoundingBox.py | 57 +++++++++++++++++++ .../gRPC/meta/gRPC_fb_getOverallTime.py | 3 +- 2 files changed, 59 insertions(+), 1 deletion(-) create mode 100755 examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py new file mode 100755 index 000000000..3d71a7615 --- /dev/null +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import os +import sys + +import flatbuffers +from fb import Boundingbox +from fb import meta_operations_grpc_fb as metaOperations + +# importing util functions. Assuming that these files are in the parent dir +# examples/python/gRPC/util.py +# examples/python/gRPC/util_fb.py +script_dir = os.path.dirname(__file__) +util_dir = os.path.join(script_dir, '..') +sys.path.append(util_dir) +import util +import util_fb + +builder = flatbuffers.Builder(1024) +# Default server is localhost ! +channel = util.get_gRPC_channel() + + +# 1. Get all projects from the server +projectuuid = util_fb.getProject(builder, channel, 'geodeticProject') + +# 2. Check if the defined project exist; if not exit +if not projectuuid: + print("Project Not Found") + exit() + +# 3. Get gRPC service object +stub = metaOperations.MetaOperationsStub(channel) + +projectInfo = util_fb.createProjectInfo(builder, "name", projectuuid) +builder.Finish(projectInfo) +buf = builder.Output() + +responseBuf = stub.GetOverallBoundingBox(bytes(buf)) +response = Boundingbox.Boundingbox.GetRootAs(responseBuf) + +print( + "Min Point (X, Y, Z): " + + str(response.PointMin().X()) + + " , " + + str(response.PointMin().Y()) + + " , " + + str(response.PointMin().Z()) +) +print( + "Max Point (X, Y, Z): " + + str(response.PointMax().X()) + + " , " + + str(response.PointMax().Y()) + + " , " + + str(response.PointMax().Z()) +) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py index 763bb1585..adb3d9e47 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py @@ -22,10 +22,11 @@ # 1. Get all projects from the server -projectuuid = util_fb.getProject(builder, channel, 'testproject') +projectuuid = util_fb.getProject(builder, channel, 'geodeticProject') # 2. Check if the defined project exist; if not exit if not projectuuid: + print("Project Not Found") exit() # 3. Get gRPC service object From cd89beb3314e75da07607a8333cc705490dc03a1 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Tue, 20 Dec 2022 11:42:27 +0000 Subject: [PATCH 06/21] switching from string as datatype to fb::datatype --- .../meta/gRPC_fb_getOverallBoundingBox.py | 6 +- examples/python/gRPC/util_fb.py | 15 +++ seerep-msgs/fbs/uuid_datatype_pair.fbs | 9 ++ seerep_com/fbs/meta_operations.fbs | 5 +- seerep_msgs/CMakeLists.txt | 1 + .../seerep_core/include/seerep_core/core.h | 5 +- .../include/seerep_core/core_dataset.h | 4 +- .../include/seerep_core/core_project.h | 4 +- seerep_srv/seerep_core/src/core.cpp | 9 +- seerep_srv/seerep_core/src/core_dataset.cpp | 29 +++++- seerep_srv/seerep_core/src/core_project.cpp | 8 +- .../seerep_server/fb_meta_operations.h | 4 +- .../seerep_server/src/fb_meta_operations.cpp | 99 ++++++++++++------- 13 files changed, 141 insertions(+), 57 deletions(-) create mode 100644 seerep-msgs/fbs/uuid_datatype_pair.fbs diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py index 3d71a7615..a89c4d634 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py @@ -22,7 +22,7 @@ # 1. Get all projects from the server -projectuuid = util_fb.getProject(builder, channel, 'geodeticProject') +projectuuid = util_fb.getProject(builder, channel, 'LabeledImagesInGrid') # 2. Check if the defined project exist; if not exit if not projectuuid: @@ -32,8 +32,8 @@ # 3. Get gRPC service object stub = metaOperations.MetaOperationsStub(channel) -projectInfo = util_fb.createProjectInfo(builder, "name", projectuuid) -builder.Finish(projectInfo) +UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, ["Image"]) +builder.Finish(UuidDatatypePair) buf = builder.Output() responseBuf = stub.GetOverallBoundingBox(bytes(buf)) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 03fb181dc..1e7bee21c 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -25,6 +25,7 @@ TimeInterval, Timestamp, TransformStampedQuery, + UuidDatatypePair, ) from seerep.fb import meta_operations_grpc_fb as metaOperations @@ -448,6 +449,20 @@ def createTransformStampedQuery(builder, header, childFrameId): return TransformStampedQuery.End(builder) +def createUuidDatatypePair(builder, uuid, datatypes): + uuidStr = builder.CreateString(uuid) + + UuidDatatypePair.StartLabelVector(builder, len(datatypes)) + for datatype in reversed(datatypes): + builder.PrependUOffsetTRelative(datatype) + datatypesOffset = builder.EndVector() + + UuidDatatypePair.Start(builder) + UuidDatatypePair.AddUuid(builder, uuidStr) + UuidDatatypePair.AddDatatypes(builder, datatypesOffset) + return UuidDatatypePair.End(builder) + + def createProjectInfo(builder, name, uuid): nameStr = builder.CreateString(name) uuidStr = builder.CreateString(uuid) diff --git a/seerep-msgs/fbs/uuid_datatype_pair.fbs b/seerep-msgs/fbs/uuid_datatype_pair.fbs new file mode 100644 index 000000000..5b1aa97ed --- /dev/null +++ b/seerep-msgs/fbs/uuid_datatype_pair.fbs @@ -0,0 +1,9 @@ + +include "datatype.fbs"; + +namespace seerep.fb; + +table UuidDatatypePair { + projectuuid:string; + datatypes:[Datatype]; +} diff --git a/seerep_com/fbs/meta_operations.fbs b/seerep_com/fbs/meta_operations.fbs index 508851abb..00bb7692c 100644 --- a/seerep_com/fbs/meta_operations.fbs +++ b/seerep_com/fbs/meta_operations.fbs @@ -6,6 +6,7 @@ include "empty.fbs"; include "boundingbox.fbs"; include "datatype.fbs"; include "time_interval.fbs"; +include "uuid_datatype_pair.fbs"; namespace seerep.fb; @@ -14,6 +15,6 @@ rpc_service MetaOperations { GetProjects(seerep.fb.Empty):seerep.fb.ProjectInfos; LoadProjects(seerep.fb.Empty):seerep.fb.Empty; DeleteProject(seerep.fb.ProjectInfo):seerep.fb.Empty; - GetOverallTimeInterval(seerep.fb.ProjectInfo):seerep.fb.TimeInterval; - GetOverallBoundingBox(seerep.fb.ProjectInfo):seerep.fb.Boundingbox; + GetOverallTimeInterval(seerep.fb.UuidDatatypePair):seerep.fb.TimeInterval; + GetOverallBoundingBox(seerep.fb.UuidDatatypePair):seerep.fb.Boundingbox; } diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 6afa5a5b1..24fe59b0b 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -112,6 +112,7 @@ set(MY_FBS_FILES fbs/union_map_entry.fbs fbs/uuids_per_project.fbs fbs/uuids_project_pair.fbs + fbs/uuid_datatype_pair.fbs fbs/vector3_stamped.fbs fbs/vector3.fbs ) diff --git a/seerep_srv/seerep_core/include/seerep_core/core.h b/seerep_srv/seerep_core/include/seerep_core/core.h index 8ac95f4e8..e3968de5b 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core.h +++ b/seerep_srv/seerep_core/include/seerep_core/core.h @@ -131,8 +131,9 @@ class Core */ void deleteProject(boost::uuids::uuid uuid); - seerep_core_msgs::AabbTime getOverallTimeInterval(boost::uuids::uuid uuid); - seerep_core_msgs::AABB getOverallBound(boost::uuids::uuid uuid); + seerep_core_msgs::AabbTime getOverallTimeInterval(boost::uuids::uuid uuid, + std::vector datatypes); + seerep_core_msgs::AABB getOverallBound(boost::uuids::uuid uuid, std::vector datatypes); private: /** diff --git a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h index a9092263e..04135366e 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -124,8 +124,8 @@ class CoreDataset * * @return seerep_core_msgs::AABB */ - seerep_core_msgs::AabbTime getTimeBounds(); - seerep_core_msgs::AABB getSpatialBounds(); + seerep_core_msgs::AabbTime getTimeBounds(std::vector datatypes); + seerep_core_msgs::AABB getSpatialBounds(std::vector datatypes); private: /** 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 6ac7afd10..191016ddc 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_project.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_project.h @@ -142,8 +142,8 @@ class CoreProject */ std::shared_ptr getHdf5File(); - seerep_core_msgs::AabbTime getTimeBounds(); - seerep_core_msgs::AABB getSpatialBounds(); + seerep_core_msgs::AabbTime getTimeBounds(std::vector datatypes); + seerep_core_msgs::AABB getSpatialBounds(std::vector datatypes); private: /** diff --git a/seerep_srv/seerep_core/src/core.cpp b/seerep_srv/seerep_core/src/core.cpp index 2e3c4e162..667d3772b 100644 --- a/seerep_srv/seerep_core/src/core.cpp +++ b/seerep_srv/seerep_core/src/core.cpp @@ -274,16 +274,17 @@ void Core::addDatasetToResult(seerep_core_msgs::QueryResultProject& dataset, see } } -seerep_core_msgs::AabbTime Core::getOverallTimeInterval(boost::uuids::uuid uuid) +seerep_core_msgs::AabbTime Core::getOverallTimeInterval(boost::uuids::uuid uuid, + std::vector datatypes) { auto project = findProject(uuid); - return project->second->getTimeBounds(); + return project->second->getTimeBounds(datatypes); } -seerep_core_msgs::AABB Core::getOverallBound(boost::uuids::uuid uuid) +seerep_core_msgs::AABB Core::getOverallBound(boost::uuids::uuid uuid, std::vector datatypes) { auto project = findProject(uuid); - return project->second->getSpatialBounds(); + return project->second->getSpatialBounds(datatypes); } } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index 7d8f46941..5f2dec991 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -517,14 +517,37 @@ void CoreDataset::addLabels(const seerep_core_msgs::Datatype& datatype, datatypeSpecifics->datasetInstancesMap.emplace(msgUuid, instanceUuids); } -seerep_core_msgs::AabbTime CoreDataset::getTimeBounds() +seerep_core_msgs::AabbTime CoreDataset::getTimeBounds(std::vector datatypes) { return m_datatypeDatatypeSpecificsMap.at(seerep_core_msgs::Datatype::Point)->timetree.bounds(); } -seerep_core_msgs::AABB CoreDataset::getSpatialBounds() +seerep_core_msgs::AABB CoreDataset::getSpatialBounds(std::vector datatypes) { - return m_datatypeDatatypeSpecificsMap.at(seerep_core_msgs::Datatype::PointCloud)->rt.bounds(); + seerep_core_msgs::AABB overallbb; + for (seerep_core_msgs::Datatype dt : datatypes) + { + seerep_core_msgs::AABB rtree_bounds = m_datatypeDatatypeSpecificsMap.at(dt)->rt.bounds(); + + if (rtree_bounds.max_corner().get<0>() > overallbb.max_corner().get<0>() || + rtree_bounds.max_corner().get<1>() > overallbb.max_corner().get<1>() || + rtree_bounds.max_corner().get<2>() > overallbb.max_corner().get<2>()) + { + overallbb.max_corner().set<0>(rtree_bounds.max_corner().get<0>()); + overallbb.max_corner().set<1>(rtree_bounds.max_corner().get<1>()); + overallbb.max_corner().set<2>(rtree_bounds.max_corner().get<2>()); + } + + if (rtree_bounds.min_corner().get<0>() < overallbb.min_corner().get<0>() || + rtree_bounds.min_corner().get<1>() < overallbb.min_corner().get<1>() || + rtree_bounds.min_corner().get<2>() < overallbb.min_corner().get<2>()) + { + overallbb.min_corner().set<0>(rtree_bounds.min_corner().get<0>()); + overallbb.min_corner().set<1>(rtree_bounds.min_corner().get<1>()); + overallbb.min_corner().set<2>(rtree_bounds.min_corner().get<2>()); + } + } + return overallbb; } } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index b4a42f713..f580da8a7 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -137,14 +137,14 @@ void CoreProject::recreateDatatypes() } } -seerep_core_msgs::AabbTime CoreProject::getTimeBounds() +seerep_core_msgs::AabbTime CoreProject::getTimeBounds(std::vector datatypes) { - return m_coreDatasets->getTimeBounds(); + return m_coreDatasets->getTimeBounds(datatypes); } -seerep_core_msgs::AABB CoreProject::getSpatialBounds() +seerep_core_msgs::AABB CoreProject::getSpatialBounds(std::vector datatypes) { - return m_coreDatasets->getSpatialBounds(); + return m_coreDatasets->getSpatialBounds(datatypes); } } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 32fd3701f..9d502bab6 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -26,10 +26,10 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request, + const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request, + const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; private: diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index d91900c42..9155a8b98 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -165,44 +165,77 @@ grpc::Status FbMetaOperations::DeleteProject(grpc::ServerContext* context, return grpc::Status::OK; } -grpc::Status FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) +// grpc::Status +// FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, +// const flatbuffers::grpc::Message* request, +// flatbuffers::grpc::Message* response) +// { +// std::string uuid = request->GetRoot()->projectuuid()->str(); +// boost::uuids::string_generator gen; +// auto uuidFromString = gen(uuid); + +// std::vector dt_vector; +// for (auto dt : *request->GetRoot()->datatypes()) +// { +// dt_vector.push_back(dt->str()); +// } + +// seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString); + +// flatbuffers::grpc::MessageBuilder builder; + +// seerep::fb::TimestampBuilder minTimeStampBuilder(builder); +// minTimeStampBuilder.add_seconds(timeinterval.min_corner().get<0>()); +// flatbuffers::Offset min = minTimeStampBuilder.Finish(); + +// seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); +// maxTimeStampBuilder.add_seconds(timeinterval.max_corner().get<0>()); +// flatbuffers::Offset max = maxTimeStampBuilder.Finish(); + +// seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); +// timeIntervalBuilder.add_time_min(min); +// timeIntervalBuilder.add_time_max(max); +// flatbuffers::Offset bb = timeIntervalBuilder.Finish(); + +// builder.Finish(bb); +// *response = builder.ReleaseMessage(); + +// return grpc::Status::OK; +// } + +grpc::Status +FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) { - std::string uuid = request->GetRoot()->uuid()->str(); + (void)context; // ignore that variable without causing warnings + + std::string uuid = request->GetRoot()->projectuuid()->str(); boost::uuids::string_generator gen; auto uuidFromString = gen(uuid); - seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString); - - flatbuffers::grpc::MessageBuilder builder; - - seerep::fb::TimestampBuilder minTimeStampBuilder(builder); - minTimeStampBuilder.add_seconds(timeinterval.min_corner().get<0>()); - flatbuffers::Offset min = minTimeStampBuilder.Finish(); - - seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); - maxTimeStampBuilder.add_seconds(timeinterval.max_corner().get<0>()); - flatbuffers::Offset max = maxTimeStampBuilder.Finish(); - - seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); - timeIntervalBuilder.add_time_min(min); - timeIntervalBuilder.add_time_max(max); - flatbuffers::Offset bb = timeIntervalBuilder.Finish(); - - builder.Finish(bb); - *response = builder.ReleaseMessage(); - return grpc::Status::OK; -} + std::vector dt_vector; + for (auto datatype : *request->GetRoot()->datatypes()) + { + if (datatype == seerep::fb::Datatype_Image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (datatype == seerep::fb::Datatype_PointCloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (datatype == seerep::fb::Datatype_Point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + } -grpc::Status FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) -{ - std::string uuid = request->GetRoot()->uuid()->str(); - boost::uuids::string_generator gen; - auto uuidFromString = gen(uuid); - seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString); + seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); flatbuffers::grpc::MessageBuilder builder; From ef7faa09b3f13f1fcc0d8b128dcf63c48dcf2430 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Thu, 5 Jan 2023 12:57:15 +0000 Subject: [PATCH 07/21] datatype send error --- .../python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py | 9 +++++++-- examples/python/gRPC/util_fb.py | 4 ++-- .../include/seerep_server/fb_meta_operations.h | 6 +++--- seerep_srv/seerep_server/src/fb_meta_operations.cpp | 6 ++++-- 4 files changed, 16 insertions(+), 9 deletions(-) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py index a89c4d634..816fb1271 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py @@ -4,7 +4,7 @@ import sys import flatbuffers -from fb import Boundingbox +from fb import Boundingbox, Datatype from fb import meta_operations_grpc_fb as metaOperations # importing util functions. Assuming that these files are in the parent dir @@ -32,7 +32,12 @@ # 3. Get gRPC service object stub = metaOperations.MetaOperationsStub(channel) -UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, ["Image"]) +# 4. Create an instance of fb.Datatype +requested_datatype = Datatype.Datatype() + +UuidDatatypePair = util_fb.createUuidDatatypePair( + builder, projectuuid, [requested_datatype.Image, requested_datatype.PointCloud] +) builder.Finish(UuidDatatypePair) buf = builder.Output() diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 1e7bee21c..2f37d469c 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -452,13 +452,13 @@ def createTransformStampedQuery(builder, header, childFrameId): def createUuidDatatypePair(builder, uuid, datatypes): uuidStr = builder.CreateString(uuid) - UuidDatatypePair.StartLabelVector(builder, len(datatypes)) + UuidDatatypePair.StartDatatypesVector(builder, len(datatypes)) for datatype in reversed(datatypes): builder.PrependUOffsetTRelative(datatype) datatypesOffset = builder.EndVector() UuidDatatypePair.Start(builder) - UuidDatatypePair.AddUuid(builder, uuidStr) + UuidDatatypePair.AddProjectuuid(builder, uuidStr) UuidDatatypePair.AddDatatypes(builder, datatypesOffset) return UuidDatatypePair.End(builder) diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 9d502bab6..2bf0600e2 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -25,9 +25,9 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service grpc::Status DeleteProject(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; - grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, - const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) override; + // grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, + // const flatbuffers::grpc::Message* request, + // flatbuffers::grpc::Message* response) override; grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 9155a8b98..17f61d41e 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -209,14 +209,16 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, flatbuffers::grpc::Message* response) { (void)context; // ignore that variable without causing warnings + auto requestRoot = request->GetRoot(); - std::string uuid = request->GetRoot()->projectuuid()->str(); + std::string uuid = requestRoot->projectuuid()->str(); boost::uuids::string_generator gen; auto uuidFromString = gen(uuid); std::vector dt_vector; - for (auto datatype : *request->GetRoot()->datatypes()) + for (auto datatype : *requestRoot->datatypes()) { + auto casted_datatype = static_cast(datatype); if (datatype == seerep::fb::Datatype_Image) { dt_vector.push_back(seerep_core_msgs::Datatype::Image); From a5e2b884b7c81290df12038e06ca221fff970c7a Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Thu, 5 Jan 2023 13:43:57 +0000 Subject: [PATCH 08/21] vector of enum is not really working disregard the vector approach for now --- .../meta/gRPC_fb_getOverallBoundingBox.py | 6 +--- examples/python/gRPC/util_fb.py | 9 ++--- seerep-msgs/fbs/uuid_datatype_pair.fbs | 2 +- .../seerep_server/src/fb_meta_operations.cpp | 34 +++++++++---------- 4 files changed, 20 insertions(+), 31 deletions(-) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py index 816fb1271..b99d36ad4 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py @@ -32,12 +32,8 @@ # 3. Get gRPC service object stub = metaOperations.MetaOperationsStub(channel) -# 4. Create an instance of fb.Datatype -requested_datatype = Datatype.Datatype() +UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, Datatype.Datatype().Image) -UuidDatatypePair = util_fb.createUuidDatatypePair( - builder, projectuuid, [requested_datatype.Image, requested_datatype.PointCloud] -) builder.Finish(UuidDatatypePair) buf = builder.Output() diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 2f37d469c..70f4e7cd8 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -449,17 +449,12 @@ def createTransformStampedQuery(builder, header, childFrameId): return TransformStampedQuery.End(builder) -def createUuidDatatypePair(builder, uuid, datatypes): +def createUuidDatatypePair(builder, uuid, datatype): uuidStr = builder.CreateString(uuid) - UuidDatatypePair.StartDatatypesVector(builder, len(datatypes)) - for datatype in reversed(datatypes): - builder.PrependUOffsetTRelative(datatype) - datatypesOffset = builder.EndVector() - UuidDatatypePair.Start(builder) UuidDatatypePair.AddProjectuuid(builder, uuidStr) - UuidDatatypePair.AddDatatypes(builder, datatypesOffset) + UuidDatatypePair.AddDatatypes(builder, datatype) return UuidDatatypePair.End(builder) diff --git a/seerep-msgs/fbs/uuid_datatype_pair.fbs b/seerep-msgs/fbs/uuid_datatype_pair.fbs index 5b1aa97ed..82b9896f8 100644 --- a/seerep-msgs/fbs/uuid_datatype_pair.fbs +++ b/seerep-msgs/fbs/uuid_datatype_pair.fbs @@ -5,5 +5,5 @@ namespace seerep.fb; table UuidDatatypePair { projectuuid:string; - datatypes:[Datatype]; + datatypes:Datatype; } diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 17f61d41e..6f7e011ab 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -216,25 +216,23 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, auto uuidFromString = gen(uuid); std::vector dt_vector; - for (auto datatype : *requestRoot->datatypes()) + + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatypes()); + if (casted_datatype == seerep::fb::Datatype_Image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (casted_datatype == seerep::fb::Datatype_PointCloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (casted_datatype == seerep::fb::Datatype_Point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else { - auto casted_datatype = static_cast(datatype); - if (datatype == seerep::fb::Datatype_Image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (datatype == seerep::fb::Datatype_PointCloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (datatype == seerep::fb::Datatype_Point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); } seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); From 8eb31c48ce6a2abe417270cdb2dd931e0ca57368 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Tue, 10 Jan 2023 17:26:43 +0000 Subject: [PATCH 09/21] correctly obtaining bounding boxes --- seerep_srv/seerep_core/src/core_dataset.cpp | 73 +++++++++++-- .../seerep_server/fb_meta_operations.h | 6 +- .../seerep_server/src/fb_meta_operations.cpp | 103 +++++++++++------- 3 files changed, 131 insertions(+), 51 deletions(-) diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index 5f2dec991..1f1c4fc45 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -519,33 +519,84 @@ void CoreDataset::addLabels(const seerep_core_msgs::Datatype& datatype, seerep_core_msgs::AabbTime CoreDataset::getTimeBounds(std::vector datatypes) { - return m_datatypeDatatypeSpecificsMap.at(seerep_core_msgs::Datatype::Point)->timetree.bounds(); + seerep_core_msgs::AabbTime overalltime; + + overalltime.max_corner().set<0>(0); + overalltime.min_corner().set<0>(std::numeric_limits::max()); + + for (seerep_core_msgs::Datatype dt : datatypes) + { + seerep_core_msgs::AabbTime timeinterval = m_datatypeDatatypeSpecificsMap.at(dt)->timetree.bounds(); + + // compare min and update if need be + if (timeinterval.min_corner().get<0>() < overalltime.min_corner().get<0>()) + { + overalltime.min_corner().set<0>(timeinterval.min_corner().get<0>()); + } + + // compare min and update if need be + if (timeinterval.max_corner().get<0>() > overalltime.max_corner().get<0>()) + { + overalltime.max_corner().set<0>(timeinterval.max_corner().get<0>()); + } + } + + return overalltime; } seerep_core_msgs::AABB CoreDataset::getSpatialBounds(std::vector datatypes) { seerep_core_msgs::AABB overallbb; + + // set the minimum to zero + overallbb.max_corner().set<0>(0); + overallbb.max_corner().set<1>(0); + overallbb.max_corner().set<2>(0); + + // set the maximum for the maximum possible for the datatype + overallbb.min_corner().set<0>(std::numeric_limits::max()); + overallbb.min_corner().set<1>(std::numeric_limits::max()); + overallbb.min_corner().set<2>(std::numeric_limits::max()); + for (seerep_core_msgs::Datatype dt : datatypes) { seerep_core_msgs::AABB rtree_bounds = m_datatypeDatatypeSpecificsMap.at(dt)->rt.bounds(); - if (rtree_bounds.max_corner().get<0>() > overallbb.max_corner().get<0>() || - rtree_bounds.max_corner().get<1>() > overallbb.max_corner().get<1>() || - rtree_bounds.max_corner().get<2>() > overallbb.max_corner().get<2>()) + // update the min if needed for dimension 0 + if (rtree_bounds.min_corner().get<0>() < overallbb.min_corner().get<0>()) { - overallbb.max_corner().set<0>(rtree_bounds.max_corner().get<0>()); - overallbb.max_corner().set<1>(rtree_bounds.max_corner().get<1>()); - overallbb.max_corner().set<2>(rtree_bounds.max_corner().get<2>()); + overallbb.min_corner().set<0>(rtree_bounds.min_corner().get<0>()); } - if (rtree_bounds.min_corner().get<0>() < overallbb.min_corner().get<0>() || - rtree_bounds.min_corner().get<1>() < overallbb.min_corner().get<1>() || - rtree_bounds.min_corner().get<2>() < overallbb.min_corner().get<2>()) + // update the min if needed for dimension 1 + if (rtree_bounds.min_corner().get<1>() < overallbb.min_corner().get<1>()) { - overallbb.min_corner().set<0>(rtree_bounds.min_corner().get<0>()); overallbb.min_corner().set<1>(rtree_bounds.min_corner().get<1>()); + } + + // update the min if needed for dimension 2 + if (rtree_bounds.min_corner().get<2>() < overallbb.min_corner().get<2>()) + { overallbb.min_corner().set<2>(rtree_bounds.min_corner().get<2>()); } + + // update the max if needed for dimension 0 + if (rtree_bounds.max_corner().get<0>() > overallbb.max_corner().get<0>()) + { + overallbb.max_corner().set<0>(rtree_bounds.max_corner().get<0>()); + } + + // update the max if needed for dimension 1 + if (rtree_bounds.max_corner().get<1>() > overallbb.max_corner().get<1>()) + { + overallbb.max_corner().set<1>(rtree_bounds.max_corner().get<1>()); + } + + // update the max if needed for dimension 2 + if (rtree_bounds.max_corner().get<2>() > overallbb.max_corner().get<2>()) + { + overallbb.max_corner().set<2>(rtree_bounds.max_corner().get<2>()); + } } return overallbb; } diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 2bf0600e2..9d502bab6 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -25,9 +25,9 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service grpc::Status DeleteProject(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; - // grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, - // const flatbuffers::grpc::Message* request, - // flatbuffers::grpc::Message* response) override; + grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) override; grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 6f7e011ab..926e5922f 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -165,43 +165,72 @@ grpc::Status FbMetaOperations::DeleteProject(grpc::ServerContext* context, return grpc::Status::OK; } -// grpc::Status -// FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, -// const flatbuffers::grpc::Message* request, -// flatbuffers::grpc::Message* response) -// { -// std::string uuid = request->GetRoot()->projectuuid()->str(); -// boost::uuids::string_generator gen; -// auto uuidFromString = gen(uuid); - -// std::vector dt_vector; -// for (auto dt : *request->GetRoot()->datatypes()) -// { -// dt_vector.push_back(dt->str()); -// } - -// seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString); - -// flatbuffers::grpc::MessageBuilder builder; - -// seerep::fb::TimestampBuilder minTimeStampBuilder(builder); -// minTimeStampBuilder.add_seconds(timeinterval.min_corner().get<0>()); -// flatbuffers::Offset min = minTimeStampBuilder.Finish(); - -// seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); -// maxTimeStampBuilder.add_seconds(timeinterval.max_corner().get<0>()); -// flatbuffers::Offset max = maxTimeStampBuilder.Finish(); - -// seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); -// timeIntervalBuilder.add_time_min(min); -// timeIntervalBuilder.add_time_max(max); -// flatbuffers::Offset bb = timeIntervalBuilder.Finish(); - -// builder.Finish(bb); -// *response = builder.ReleaseMessage(); - -// return grpc::Status::OK; -// } +grpc::Status +FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) +{ + (void)context; // ignore that variable without causing warnings + auto requestRoot = request->GetRoot(); + + std::string uuid = request->GetRoot()->projectuuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector dt_vector; + + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatypes()); + if (casted_datatype == seerep::fb::Datatype_Image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (casted_datatype == seerep::fb::Datatype_PointCloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (casted_datatype == seerep::fb::Datatype_Point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString, dt_vector); + + // isolate second and nano second bits from min time + uint64_t mintime = timeinterval.min_corner().get<0>(); + uint32_t min_nanos = (uint32_t)mintime; + uint32_t min_seconds = (uint32_t)(mintime >> 32); + + // isolate second and nano second bits from max time + uint64_t maxtime = timeinterval.max_corner().get<0>(); + uint32_t max_nanos = (uint32_t)maxtime; + uint32_t max_seconds = (uint32_t)(maxtime >> 32); + + flatbuffers::grpc::MessageBuilder builder; + + seerep::fb::TimestampBuilder minTimeStampBuilder(builder); + minTimeStampBuilder.add_seconds(min_seconds); + minTimeStampBuilder.add_nanos(min_nanos); + flatbuffers::Offset min = minTimeStampBuilder.Finish(); + + seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); + maxTimeStampBuilder.add_seconds(max_seconds); + maxTimeStampBuilder.add_nanos(max_nanos); + flatbuffers::Offset max = maxTimeStampBuilder.Finish(); + + seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); + timeIntervalBuilder.add_time_min(min); + timeIntervalBuilder.add_time_max(max); + flatbuffers::Offset bb = timeIntervalBuilder.Finish(); + + builder.Finish(bb); + *response = builder.ReleaseMessage(); + + return grpc::Status::OK; +} grpc::Status FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, From 0b45f764921529b07476bd58a2acc0ca1c06823d Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Tue, 10 Jan 2023 17:27:19 +0000 Subject: [PATCH 10/21] working python example --- .../python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py index b99d36ad4..4c31aaa6d 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py @@ -4,7 +4,7 @@ import sys import flatbuffers -from fb import Boundingbox, Datatype +from fb import Boundingbox, Datatype, TimeInterval from fb import meta_operations_grpc_fb as metaOperations # importing util functions. Assuming that these files are in the parent dir @@ -56,3 +56,10 @@ + " , " + str(response.PointMax().Z()) ) + +responseBuf = stub.GetOverallTimeInterval(bytes(buf)) +response = TimeInterval.TimeInterval.GetRootAs(responseBuf) + +print("Min Time (Seconds, Nanoseconds): " + str(response.TimeMin().Seconds()) + " ," + str(response.TimeMin().Nanos())) + +print("Max Time (Seconds, Nanoseconds): " + str(response.TimeMax().Seconds()) + " ," + str(response.TimeMax().Nanos())) From 37898f6212f813f603e6429f59a374ac17f6444f Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Tue, 24 Jan 2023 09:13:03 +0000 Subject: [PATCH 11/21] protobuf implementation --- seerep-msgs/protos/datatype.proto | 11 +++ seerep-msgs/protos/uuid_datatype_pair.proto | 11 +++ seerep_com/protos/meta_operations.proto | 5 ++ seerep_msgs/CMakeLists.txt | 2 + .../seerep_server/pb_meta_operations.h | 4 + .../seerep_server/src/pb_meta_operations.cpp | 85 +++++++++++++++++++ 6 files changed, 118 insertions(+) create mode 100644 seerep-msgs/protos/datatype.proto create mode 100644 seerep-msgs/protos/uuid_datatype_pair.proto diff --git a/seerep-msgs/protos/datatype.proto b/seerep-msgs/protos/datatype.proto new file mode 100644 index 000000000..ff199dfe8 --- /dev/null +++ b/seerep-msgs/protos/datatype.proto @@ -0,0 +1,11 @@ +syntax = "proto3"; + +package seerep; + +enum datatype +{ + unknown = 0; + image = 1; + pointcloud = 2; + point = 3; +} diff --git a/seerep-msgs/protos/uuid_datatype_pair.proto b/seerep-msgs/protos/uuid_datatype_pair.proto new file mode 100644 index 000000000..a41dc8221 --- /dev/null +++ b/seerep-msgs/protos/uuid_datatype_pair.proto @@ -0,0 +1,11 @@ +syntax = "proto3"; + +package seerep; + +import "datatype.proto"; + +message UuidDatatypePair +{ + string projectuuid = 1; + datatype datatype = 2; +} diff --git a/seerep_com/protos/meta_operations.proto b/seerep_com/protos/meta_operations.proto index 7136ec6dd..6db06249b 100644 --- a/seerep_com/protos/meta_operations.proto +++ b/seerep_com/protos/meta_operations.proto @@ -7,9 +7,14 @@ import "google/protobuf/empty.proto"; import "project_info.proto"; import "project_infos.proto"; import "projectCreation.proto"; +import "uuid_datatype_pair.proto"; +import "time_interval.proto"; +import "boundingbox.proto"; service MetaOperations { rpc CreateProject(ProjectCreation) returns (ProjectInfo); rpc GetProjects(google.protobuf.Empty) returns (ProjectInfos); + rpc GetOverallTimeInterval(UuidDatatypePair) returns (TimeInterval); + rpc GetOverallBoundingBox(UuidDatatypePair) returns (Boundingbox); } diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 24fe59b0b..5b992e9a1 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(MY_PROTO_FILES protos/boundingbox2d_labeled_with_category.proto protos/boundingbox2d_stamped.proto protos/boundingbox2d.proto + protos/datatype.proto protos/frame_infos.proto protos/frame_query.proto protos/geodetic_coordinates.proto @@ -57,6 +58,7 @@ set(MY_PROTO_FILES protos/twist_with_covariance_stamped.proto protos/twist_with_covariance.proto protos/twist.proto + protos/uuid_datatype_pair.proto protos/vector3_stamped.proto protos/vector3.proto ) diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h index 4ba20831a..13be59208 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h @@ -22,6 +22,10 @@ class PbMetaOperations final : public seerep::pb::MetaOperations::Service seerep::pb::ProjectInfo* response); grpc::Status GetProjects(grpc::ServerContext* context, const google::protobuf::Empty* request, seerep::pb::ProjectInfos* response); + grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, + seerep::ProjectInfos* response); + grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, + seerep::ProjectInfos* response); private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 07d9889be..92118ce54 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -94,4 +94,89 @@ grpc::Status PbMetaOperations::GetProjects(grpc::ServerContext* context, const g return grpc::Status::OK; } +grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, + seerep::TimeInterval* response) +{ + (void)context; // ignore that variable without causing warnings + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall time interval"; + + std::string uuid = request->projectuuid(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector dt_vector; + + if (request->datatype() == seerep::datatype::image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (request->datatype() == seerep::datatype::pointcloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (request->datatype() == seerep::datatype::point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString, dt_vector); + + // isolate second and nano second bits from min time + uint64_t mintime = timeinterval.min_corner().get<0>(); + uint32_t min_nanos = (uint32_t)mintime; + uint32_t min_seconds = (uint32_t)(mintime >> 32); + + // isolate second and nano second bits from max time + uint64_t maxtime = timeinterval.max_corner().get<0>(); + uint32_t max_nanos = (uint32_t)maxtime; + uint32_t max_seconds = (uint32_t)(maxtime >> 32); + + response->set_time_min(mintime); + response->set_time_max(maxtime); + + return grpc::Status::OK; +} +grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, + seerep::Boundingbox* response) +{ + (void)context; // ignore that variable without causing warnings + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; + + std::string uuid = request->projectuuid(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector dt_vector; + + if (request->datatype() == seerep::datatype::image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (request->datatype() == seerep::datatype::pointcloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (request->datatype() == seerep::datatype::point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); + + flatbuffers::grpc::MessageBuilder builder; + + response->set_min_corner(overallBB.min_corner()); + response->set_max_corner(overallBB.max_corner()); + + return grpc::Status::OK; +} + } /* namespace seerep_server */ From 1827a7542e053ab5a9db1f463b697792e657d027 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Thu, 27 Apr 2023 16:13:19 +0200 Subject: [PATCH 12/21] protobuf interface with working example --- .isort.cfg | 2 +- .../gRPC/meta/gRPC_pb_getOverallBound.py | 35 +++++++++++++++++++ .../python/gRPC/meta/gRPC_pb_getProjects.py | 1 - .../seerep_server/pb_meta_operations.h | 4 +-- .../seerep_server/src/pb_meta_operations.cpp | 26 +++++++++----- 5 files changed, 56 insertions(+), 12 deletions(-) create mode 100755 examples/python/gRPC/meta/gRPC_pb_getOverallBound.py diff --git a/.isort.cfg b/.isort.cfg index b4d08537e..8b92fe5ca 100644 --- a/.isort.cfg +++ b/.isort.cfg @@ -1,3 +1,3 @@ [settings] -known_third_party = cv2,flatbuffers,google,grpc,imageio,matplotlib,numpy,quaternion,rospy,seerep,sensor_msgs,setuptools,std_msgs,tf,util,util_fb,util_visualize,yaml +known_third_party = cv2,datatype_pb2,fb,flatbuffers,google,grpc,imageio,matplotlib,meta_operations_pb2_grpc,numpy,quaternion,rospy,seerep,sensor_msgs,setuptools,std_msgs,tf,util,util_fb,util_visualize,uuid_datatype_pair_pb2,yaml profile =black diff --git a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py new file mode 100755 index 000000000..f99901e15 --- /dev/null +++ b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 + +import os +import sys + +import datatype_pb2 as datatype +import meta_operations_pb2_grpc as metaOperations +import uuid_datatype_pair_pb2 as uuid_datatype_pair +from google.protobuf import empty_pb2 + +script_dir = os.path.dirname(__file__) +util_dir = os.path.join(script_dir, '..') +sys.path.append(util_dir) +import util + +channel = util.get_gRPC_channel() + +stub = metaOperations.MetaOperationsStub(channel) + +response = stub.GetProjects(empty_pb2.Empty()) + +# 3. Check if we have an existing test project, if not, we stop here +projectuuid = "" +for project in response.projects: + print(project.name + " " + project.uuid + "\n") + if project.name == "LabeledImagesInGrid": + projectuuid = project.uuid + +uuiddt = uuid_datatype_pair.UuidDatatypePair() +uuiddt.projectuuid = projectuuid +uuiddt.datatype = datatype.image + +response = stub.GetOverallTimeInterval(uuiddt) + +print(response) diff --git a/examples/python/gRPC/meta/gRPC_pb_getProjects.py b/examples/python/gRPC/meta/gRPC_pb_getProjects.py index caff82675..cf1d2ed0a 100755 --- a/examples/python/gRPC/meta/gRPC_pb_getProjects.py +++ b/examples/python/gRPC/meta/gRPC_pb_getProjects.py @@ -17,7 +17,6 @@ response = stub.GetProjects(empty_pb2.Empty()) - print("The server has the following projects (name/uuid):") for projectinfo in response.projects: print("\t" + projectinfo.name + " " + projectinfo.uuid) diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h index 13be59208..b443d237d 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h @@ -23,9 +23,9 @@ class PbMetaOperations final : public seerep::pb::MetaOperations::Service grpc::Status GetProjects(grpc::ServerContext* context, const google::protobuf::Empty* request, seerep::pb::ProjectInfos* response); grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, - seerep::ProjectInfos* response); + seerep::TimeInterval* response); grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, - seerep::ProjectInfos* response); + seerep::Boundingbox* response); private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 92118ce54..d83467075 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -94,8 +94,9 @@ grpc::Status PbMetaOperations::GetProjects(grpc::ServerContext* context, const g return grpc::Status::OK; } -grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, - seerep::TimeInterval* response) +grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, + const seerep::UuidDatatypePair* request, + seerep::TimeInterval* response) { (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall time interval"; @@ -135,13 +136,17 @@ grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep:: uint32_t max_nanos = (uint32_t)maxtime; uint32_t max_seconds = (uint32_t)(maxtime >> 32); - response->set_time_min(mintime); - response->set_time_max(maxtime); + response->mutable_time_min()->set_nanos(min_nanos); + response->mutable_time_min()->set_seconds(min_seconds); + + response->mutable_time_max()->set_nanos(max_nanos); + response->mutable_time_max()->set_seconds(max_seconds); return grpc::Status::OK; } -grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, - seerep::Boundingbox* response) +grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, + const seerep::UuidDatatypePair* request, + seerep::Boundingbox* response) { (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; @@ -173,8 +178,13 @@ grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::U flatbuffers::grpc::MessageBuilder builder; - response->set_min_corner(overallBB.min_corner()); - response->set_max_corner(overallBB.max_corner()); + response->mutable_point_min()->set_x(overallBB.min_corner().get<0>()); + response->mutable_point_min()->set_y(overallBB.min_corner().get<1>()); + response->mutable_point_min()->set_z(overallBB.min_corner().get<2>()); + + response->mutable_point_max()->set_x(overallBB.max_corner().get<0>()); + response->mutable_point_max()->set_y(overallBB.max_corner().get<1>()); + response->mutable_point_max()->set_z(overallBB.max_corner().get<2>()); return grpc::Status::OK; } From ef5cf69c791a27162b6fbb375fc70c2e4ddfa2c5 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Thu, 27 Apr 2023 16:19:38 +0200 Subject: [PATCH 13/21] changes after pr review --- .../meta/gRPC_fb_getOverallBoundingBox.py | 9 +- .../gRPC/meta/gRPC_fb_getOverallTime.py | 9 +- examples/python/gRPC/util_fb.py | 2 +- seerep-msgs/fbs/uuid_datatype_pair.fbs | 2 +- seerep_com/fbs/meta_operations.fbs | 1 - .../seerep_core/include/seerep_core/core.h | 13 ++ .../include/seerep_core/core_dataset.h | 12 +- .../seerep_server/src/fb_meta_operations.cpp | 172 ++++++++++++------ .../seerep_server/src/pb_meta_operations.cpp | 92 +++++++--- 9 files changed, 210 insertions(+), 102 deletions(-) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py index 4c31aaa6d..b99d36ad4 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py @@ -4,7 +4,7 @@ import sys import flatbuffers -from fb import Boundingbox, Datatype, TimeInterval +from fb import Boundingbox, Datatype from fb import meta_operations_grpc_fb as metaOperations # importing util functions. Assuming that these files are in the parent dir @@ -56,10 +56,3 @@ + " , " + str(response.PointMax().Z()) ) - -responseBuf = stub.GetOverallTimeInterval(bytes(buf)) -response = TimeInterval.TimeInterval.GetRootAs(responseBuf) - -print("Min Time (Seconds, Nanoseconds): " + str(response.TimeMin().Seconds()) + " ," + str(response.TimeMin().Nanos())) - -print("Max Time (Seconds, Nanoseconds): " + str(response.TimeMax().Seconds()) + " ," + str(response.TimeMax().Nanos())) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py index adb3d9e47..8927fc2a0 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py @@ -4,7 +4,7 @@ import sys import flatbuffers -from fb import TimeInterval +from fb import Datatype, TimeInterval from fb import meta_operations_grpc_fb as metaOperations # importing util functions. Assuming that these files are in the parent dir @@ -22,7 +22,7 @@ # 1. Get all projects from the server -projectuuid = util_fb.getProject(builder, channel, 'geodeticProject') +projectuuid = util_fb.getProject(builder, channel, 'LabeledImagesInGrid') # 2. Check if the defined project exist; if not exit if not projectuuid: @@ -32,8 +32,9 @@ # 3. Get gRPC service object stub = metaOperations.MetaOperationsStub(channel) -projectInfo = util_fb.createProjectInfo(builder, "name", projectuuid) -builder.Finish(projectInfo) +UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, Datatype.Datatype().Image) + +builder.Finish(UuidDatatypePair) buf = builder.Output() responseBuf = stub.GetOverallTimeInterval(bytes(buf)) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 70f4e7cd8..81489ee0a 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -454,7 +454,7 @@ def createUuidDatatypePair(builder, uuid, datatype): UuidDatatypePair.Start(builder) UuidDatatypePair.AddProjectuuid(builder, uuidStr) - UuidDatatypePair.AddDatatypes(builder, datatype) + UuidDatatypePair.AddDatatype(builder, datatype) return UuidDatatypePair.End(builder) diff --git a/seerep-msgs/fbs/uuid_datatype_pair.fbs b/seerep-msgs/fbs/uuid_datatype_pair.fbs index 82b9896f8..37eb18179 100644 --- a/seerep-msgs/fbs/uuid_datatype_pair.fbs +++ b/seerep-msgs/fbs/uuid_datatype_pair.fbs @@ -5,5 +5,5 @@ namespace seerep.fb; table UuidDatatypePair { projectuuid:string; - datatypes:Datatype; + datatype:Datatype; } diff --git a/seerep_com/fbs/meta_operations.fbs b/seerep_com/fbs/meta_operations.fbs index 00bb7692c..51630e18f 100644 --- a/seerep_com/fbs/meta_operations.fbs +++ b/seerep_com/fbs/meta_operations.fbs @@ -4,7 +4,6 @@ include "project_infos.fbs"; include "projectCreation.fbs"; include "empty.fbs"; include "boundingbox.fbs"; -include "datatype.fbs"; include "time_interval.fbs"; include "uuid_datatype_pair.fbs"; diff --git a/seerep_srv/seerep_core/include/seerep_core/core.h b/seerep_srv/seerep_core/include/seerep_core/core.h index e3968de5b..2999169f9 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core.h +++ b/seerep_srv/seerep_core/include/seerep_core/core.h @@ -131,8 +131,21 @@ class Core */ void deleteProject(boost::uuids::uuid uuid); + /** + * @brief Get the minimum and maximum time interval for a dataset + * @param uuid UUID of a dataset + * @param datatypes A vector of datatypes for which the time bound has to be computed + * @return seerep_core_msgs::AabbTime + */ seerep_core_msgs::AabbTime getOverallTimeInterval(boost::uuids::uuid uuid, std::vector datatypes); + + /** + * @brief Get the minimum and maximum spatial bound for a dataset + * @param uuid UUID of a dataset + * @param datatypes A vector of datatypes for which the spatial bound has to be computed + * @return seerep_core_msgs::AABB + */ seerep_core_msgs::AABB getOverallBound(boost::uuids::uuid uuid, std::vector datatypes); private: diff --git a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h index 04135366e..0e91170da 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -120,11 +120,17 @@ class CoreDataset const boost::uuids::uuid& msgUuid); /** - * @brief Get the Time Bounds object - * - * @return seerep_core_msgs::AABB + * @brief Get the minimum and maximum time interval for a dataset + * @param datatypes A vector of datatypes for which the time bound has to be computed + * @return seerep_core_msgs::AabbTime */ seerep_core_msgs::AabbTime getTimeBounds(std::vector datatypes); + + /** + * @brief Get the minimum and maximum spatial bound for a dataset + * @param datatypes A vector of datatypes for which the spatial bound has to be computed + * @return seerep_core_msgs::AABB + */ seerep_core_msgs::AABB getSpatialBounds(std::vector datatypes); private: diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 926e5922f..b4cb9c27a 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -173,13 +173,16 @@ FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, (void)context; // ignore that variable without causing warnings auto requestRoot = request->GetRoot(); - std::string uuid = request->GetRoot()->projectuuid()->str(); - boost::uuids::string_generator gen; - auto uuidFromString = gen(uuid); - + // It was not possible to send a vector of enum of datatypes + // through to this function. Therefore, on the flatbuffer service + // level, the UuidDatatypePair implementation has one datatype, + // while below this layer, in the seerep core, the implementation + // is a vector. We intend to make them consistent in the future + // by implementing a vector of enums on the flatbuffer (and protobuf) + // levels. std::vector dt_vector; - seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatypes()); + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatype()); if (casted_datatype == seerep::fb::Datatype_Image) { dt_vector.push_back(seerep_core_msgs::Datatype::Image); @@ -197,37 +200,65 @@ FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); } - seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString, dt_vector); + try + { + std::string uuid = request->GetRoot()->projectuuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); - // isolate second and nano second bits from min time - uint64_t mintime = timeinterval.min_corner().get<0>(); - uint32_t min_nanos = (uint32_t)mintime; - uint32_t min_seconds = (uint32_t)(mintime >> 32); + seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString, dt_vector); - // isolate second and nano second bits from max time - uint64_t maxtime = timeinterval.max_corner().get<0>(); - uint32_t max_nanos = (uint32_t)maxtime; - uint32_t max_seconds = (uint32_t)(maxtime >> 32); + // isolate second and nano second bits from min time + uint64_t mintime = timeinterval.min_corner().get<0>(); + uint32_t min_nanos = (uint32_t)mintime; + uint32_t min_seconds = (uint32_t)(mintime >> 32); - flatbuffers::grpc::MessageBuilder builder; + // isolate second and nano second bits from max time + uint64_t maxtime = timeinterval.max_corner().get<0>(); + uint32_t max_nanos = (uint32_t)maxtime; + uint32_t max_seconds = (uint32_t)(maxtime >> 32); - seerep::fb::TimestampBuilder minTimeStampBuilder(builder); - minTimeStampBuilder.add_seconds(min_seconds); - minTimeStampBuilder.add_nanos(min_nanos); - flatbuffers::Offset min = minTimeStampBuilder.Finish(); + flatbuffers::grpc::MessageBuilder builder; - seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); - maxTimeStampBuilder.add_seconds(max_seconds); - maxTimeStampBuilder.add_nanos(max_nanos); - flatbuffers::Offset max = maxTimeStampBuilder.Finish(); + seerep::fb::TimestampBuilder minTimeStampBuilder(builder); + minTimeStampBuilder.add_seconds(min_seconds); + minTimeStampBuilder.add_nanos(min_nanos); + flatbuffers::Offset min = minTimeStampBuilder.Finish(); - seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); - timeIntervalBuilder.add_time_min(min); - timeIntervalBuilder.add_time_max(max); - flatbuffers::Offset bb = timeIntervalBuilder.Finish(); + seerep::fb::TimestampBuilder maxTimeStampBuilder(builder); + maxTimeStampBuilder.add_seconds(max_seconds); + maxTimeStampBuilder.add_nanos(max_nanos); + flatbuffers::Offset max = maxTimeStampBuilder.Finish(); - builder.Finish(bb); - *response = builder.ReleaseMessage(); + seerep::fb::TimeIntervalBuilder timeIntervalBuilder(builder); + timeIntervalBuilder.add_time_min(min); + timeIntervalBuilder.add_time_max(max); + flatbuffers::Offset bb = timeIntervalBuilder.Finish(); + + builder.Finish(bb); + *response = builder.ReleaseMessage(); + } + catch (std::runtime_error const& e) + { + // mainly catching "invalid uuid string" when transforming uuid_project from string to uuid + // also catching core doesn't have project with uuid error + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } return grpc::Status::OK; } @@ -240,13 +271,16 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, (void)context; // ignore that variable without causing warnings auto requestRoot = request->GetRoot(); - std::string uuid = requestRoot->projectuuid()->str(); - boost::uuids::string_generator gen; - auto uuidFromString = gen(uuid); - + // It was not possible to send a vector of enum of datatypes + // through to this function. Therefore, on the flatbuffer service + // level, the UuidDatatypePair implementation has one datatype, + // while below this layer, in the seerep core, the implementation + // is a vector. We intend to make them consistent in the future + // by implementing a vector of enums on the flatbuffer (and protobuf) + // levels. std::vector dt_vector; - seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatypes()); + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatype()); if (casted_datatype == seerep::fb::Datatype_Image) { dt_vector.push_back(seerep_core_msgs::Datatype::Image); @@ -264,29 +298,57 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); } - seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); + try + { + std::string uuid = requestRoot->projectuuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); - flatbuffers::grpc::MessageBuilder builder; + seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); + + flatbuffers::grpc::MessageBuilder builder; + + seerep::fb::PointBuilder minPointBuilder(builder); + minPointBuilder.add_x(overallBB.min_corner().get<0>()); + minPointBuilder.add_y(overallBB.min_corner().get<1>()); + minPointBuilder.add_z(overallBB.min_corner().get<2>()); + flatbuffers::Offset minPoint = minPointBuilder.Finish(); + + seerep::fb::PointBuilder maxPointBuilder(builder); + maxPointBuilder.add_x(overallBB.max_corner().get<0>()); + maxPointBuilder.add_y(overallBB.max_corner().get<1>()); + maxPointBuilder.add_z(overallBB.max_corner().get<2>()); + flatbuffers::Offset maxPoint = maxPointBuilder.Finish(); + + seerep::fb::BoundingboxBuilder boundingBoxBuilder(builder); + boundingBoxBuilder.add_point_min(minPoint); + boundingBoxBuilder.add_point_max(maxPoint); + flatbuffers::Offset bb = boundingBoxBuilder.Finish(); - seerep::fb::PointBuilder minPointBuilder(builder); - minPointBuilder.add_x(overallBB.min_corner().get<0>()); - minPointBuilder.add_y(overallBB.min_corner().get<1>()); - minPointBuilder.add_z(overallBB.min_corner().get<2>()); - flatbuffers::Offset minPoint = minPointBuilder.Finish(); - - seerep::fb::PointBuilder maxPointBuilder(builder); - maxPointBuilder.add_x(overallBB.max_corner().get<0>()); - maxPointBuilder.add_y(overallBB.max_corner().get<1>()); - maxPointBuilder.add_z(overallBB.max_corner().get<2>()); - flatbuffers::Offset maxPoint = maxPointBuilder.Finish(); - - seerep::fb::BoundingboxBuilder boundingBoxBuilder(builder); - boundingBoxBuilder.add_point_min(minPoint); - boundingBoxBuilder.add_point_max(maxPoint); - flatbuffers::Offset bb = boundingBoxBuilder.Finish(); - - builder.Finish(bb); - *response = builder.ReleaseMessage(); + builder.Finish(bb); + *response = builder.ReleaseMessage(); + } + catch (std::runtime_error const& e) + { + // mainly catching "invalid uuid string" when transforming uuid_project from string to uuid + // also catching core doesn't have project with uuid error + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } return grpc::Status::OK; } diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index d83467075..ac2fa5dd8 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -101,10 +101,6 @@ grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* conte (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall time interval"; - std::string uuid = request->projectuuid(); - boost::uuids::string_generator gen; - auto uuidFromString = gen(uuid); - std::vector dt_vector; if (request->datatype() == seerep::datatype::image) @@ -124,23 +120,44 @@ grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* conte dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); } - seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString, dt_vector); + try + { + std::string uuid = request->projectuuid(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); - // isolate second and nano second bits from min time - uint64_t mintime = timeinterval.min_corner().get<0>(); - uint32_t min_nanos = (uint32_t)mintime; - uint32_t min_seconds = (uint32_t)(mintime >> 32); + seerep_core_msgs::AabbTime timeinterval = seerepCore->getOverallTimeInterval(uuidFromString, dt_vector); - // isolate second and nano second bits from max time - uint64_t maxtime = timeinterval.max_corner().get<0>(); - uint32_t max_nanos = (uint32_t)maxtime; - uint32_t max_seconds = (uint32_t)(maxtime >> 32); + // isolate second and nano second bits from min time + uint64_t mintime = timeinterval.min_corner().get<0>(); + uint32_t min_nanos = (uint32_t)mintime; + uint32_t min_seconds = (uint32_t)(mintime >> 32); - response->mutable_time_min()->set_nanos(min_nanos); - response->mutable_time_min()->set_seconds(min_seconds); + // isolate second and nano second bits from max time + uint64_t maxtime = timeinterval.max_corner().get<0>(); + uint32_t max_nanos = (uint32_t)maxtime; + uint32_t max_seconds = (uint32_t)(maxtime >> 32); - response->mutable_time_max()->set_nanos(max_nanos); - response->mutable_time_max()->set_seconds(max_seconds); + response->mutable_time_min()->set_nanos(min_nanos); + response->mutable_time_min()->set_seconds(min_seconds); + + response->mutable_time_max()->set_nanos(max_nanos); + response->mutable_time_max()->set_seconds(max_seconds); + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } return grpc::Status::OK; } @@ -151,10 +168,6 @@ grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* contex (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; - std::string uuid = request->projectuuid(); - boost::uuids::string_generator gen; - auto uuidFromString = gen(uuid); - std::vector dt_vector; if (request->datatype() == seerep::datatype::image) @@ -174,17 +187,38 @@ grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* contex dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); } - seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); + try + { + std::string uuid = request->projectuuid(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); - flatbuffers::grpc::MessageBuilder builder; + seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); - response->mutable_point_min()->set_x(overallBB.min_corner().get<0>()); - response->mutable_point_min()->set_y(overallBB.min_corner().get<1>()); - response->mutable_point_min()->set_z(overallBB.min_corner().get<2>()); + flatbuffers::grpc::MessageBuilder builder; - response->mutable_point_max()->set_x(overallBB.max_corner().get<0>()); - response->mutable_point_max()->set_y(overallBB.max_corner().get<1>()); - response->mutable_point_max()->set_z(overallBB.max_corner().get<2>()); + response->mutable_point_min()->set_x(overallBB.min_corner().get<0>()); + response->mutable_point_min()->set_y(overallBB.min_corner().get<1>()); + response->mutable_point_min()->set_z(overallBB.min_corner().get<2>()); + + response->mutable_point_max()->set_x(overallBB.max_corner().get<0>()); + response->mutable_point_max()->set_y(overallBB.max_corner().get<1>()); + response->mutable_point_max()->set_z(overallBB.max_corner().get<2>()); + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } return grpc::Status::OK; } From b099cadfa590bfd36b1c871c058ce23f55b4c0c4 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 25 Jan 2023 18:52:42 +0000 Subject: [PATCH 14/21] isort fix --- .../python/ROS/ROS_publishSimplePointcloud.py | 1 - .../gRPC/images/gRPC_pb_sendLabeledImage.py | 12 +++---- .../images/gRPC_pb_sendLabeledImageGrid.py | 12 +++---- ...gRPC_fb_sendPointsBasedOnImageInstances.py | 14 ++------ .../gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 22 ++++-------- .../loadSimulatedDataWithInstancePosition.py | 22 +++--------- examples/python/gRPC/util_fb.py | 36 +++++-------------- 7 files changed, 35 insertions(+), 84 deletions(-) diff --git a/examples/python/ROS/ROS_publishSimplePointcloud.py b/examples/python/ROS/ROS_publishSimplePointcloud.py index f2ccf9d37..21465758d 100755 --- a/examples/python/ROS/ROS_publishSimplePointcloud.py +++ b/examples/python/ROS/ROS_publishSimplePointcloud.py @@ -7,7 +7,6 @@ import rospy from sensor_msgs import point_cloud2 - # PointCloud2 color cube # https://answers.ros.org/question/289576/understanding-the-bytes-in-a-pcl2-message/ from sensor_msgs.msg import PointCloud2, PointField diff --git a/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py b/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py index 1c1e6206e..3e2f570f0 100755 --- a/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py +++ b/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py @@ -8,15 +8,15 @@ import numpy as np from google.protobuf import empty_pb2 from seerep.pb import boundingbox2d_labeled_pb2 as boundingbox2d_labeled -from seerep.pb import ( - boundingbox2d_labeled_with_category_pb2 as boundingbox2d_labeled_with_category, -) +from seerep.pb import \ + boundingbox2d_labeled_with_category_pb2 as \ + boundingbox2d_labeled_with_category from seerep.pb import image_pb2 as image from seerep.pb import image_service_pb2_grpc as imageService from seerep.pb import label_with_instance_pb2 as labelWithInstance -from seerep.pb import ( - labels_with_instance_with_category_pb2 as labels_with_instance_with_category, -) +from seerep.pb import \ + labels_with_instance_with_category_pb2 as \ + labels_with_instance_with_category from seerep.pb import meta_operations_pb2_grpc as metaOperations from seerep.pb import projectCreation_pb2 as projectCreation from seerep.pb import tf_service_pb2_grpc as tfService diff --git a/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py b/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py index 5f546b770..8b204e1e6 100755 --- a/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py +++ b/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py @@ -7,15 +7,15 @@ import numpy as np from seerep.pb import boundingbox2d_labeled_pb2 as boundingbox2d_labeled -from seerep.pb import ( - boundingbox2d_labeled_with_category_pb2 as boundingbox2d_labeled_with_category, -) +from seerep.pb import \ + boundingbox2d_labeled_with_category_pb2 as \ + boundingbox2d_labeled_with_category from seerep.pb import image_pb2 as image from seerep.pb import image_service_pb2_grpc as imageService from seerep.pb import label_with_instance_pb2 as labelWithInstance -from seerep.pb import ( - labels_with_instance_with_category_pb2 as labels_with_instance_with_category, -) +from seerep.pb import \ + labels_with_instance_with_category_pb2 as \ + labels_with_instance_with_category from seerep.pb import meta_operations_pb2_grpc as metaOperations from seerep.pb import projectCreation_pb2 as projectCreation from seerep.pb import tf_service_pb2_grpc as tfService diff --git a/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py b/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py index 85ca64fa5..c14d1e2e4 100755 --- a/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py +++ b/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py @@ -4,18 +4,8 @@ import sys import flatbuffers -from seerep.fb import ( - Datatypes, - Header, - Image, - Integer, - LabelWithInstance, - Point, - PointStamped, - String, - Timestamp, - UnionMapEntry, -) +from seerep.fb import (Datatypes, Header, Image, Integer, LabelWithInstance, + Point, PointStamped, String, Timestamp, UnionMapEntry) from seerep.fb import image_service_grpc_fb as imageService from seerep.fb import point_service_grpc_fb as pointService diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index 02d947a83..f874d676e 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -17,21 +17,13 @@ sys.path.append(util_dir) import util -from util_fb import ( - addToBoundingBoxLabeledVector, - addToGeneralLabelsVector, - addToPointFieldVector, - createBoundingBoxes, - createBoundingBoxesLabeled, - createBoundingBoxLabeledWithCategory, - createHeader, - createLabelsWithInstance, - createLabelWithCategory, - createPoint, - createPointFields, - createTimeStamp, - getOrCreateProject, -) +from util_fb import (addToBoundingBoxLabeledVector, addToGeneralLabelsVector, + addToPointFieldVector, createBoundingBoxes, + createBoundingBoxesLabeled, + createBoundingBoxLabeledWithCategory, createHeader, + createLabelsWithInstance, createLabelWithCategory, + createPoint, createPointFields, createTimeStamp, + getOrCreateProject) NUM_GENERAL_LABELS = 1 NUM_BB_LABELS = 1 diff --git a/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py b/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py index 33a321bbe..0b480ee5a 100755 --- a/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py +++ b/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py @@ -8,23 +8,11 @@ import numpy as np import quaternion import yaml -from seerep.fb import ( - Boundingbox2D, - BoundingBox2DLabeled, - BoundingBox2DLabeledWithCategory, - Header, - Image, - LabelsWithCategory, - LabelWithInstance, - Point, - Point2D, - PointStamped, - Quaternion, - Timestamp, - Transform, - TransformStamped, - Vector3, -) +from seerep.fb import (Boundingbox2D, BoundingBox2DLabeled, + BoundingBox2DLabeledWithCategory, Header, Image, + LabelsWithCategory, LabelWithInstance, Point, Point2D, + PointStamped, Quaternion, Timestamp, Transform, + TransformStamped, Vector3) from seerep.fb import image_service_grpc_fb as imageService from seerep.fb import point_service_grpc_fb as pointService from seerep.fb import tf_service_grpc_fb as tfService diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 81489ee0a..da37bdf06 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -1,32 +1,14 @@ import sys -from seerep.fb import ( - Boundingbox, - BoundingBox2DLabeled, - BoundingBox2DLabeledWithCategory, - BoundingBoxes2DLabeledStamped, - BoundingBoxLabeled, - BoundingBoxLabeledWithCategory, - BoundingboxStamped, - Empty, - GeodeticCoordinates, - Header, - Label, - LabelsWithCategory, - LabelWithInstance, - Point, - PointCloud2, - PointField, - ProjectCreation, - ProjectInfo, - ProjectInfos, - Query, - QueryInstance, - TimeInterval, - Timestamp, - TransformStampedQuery, - UuidDatatypePair, -) +from seerep.fb import (Boundingbox, BoundingBox2DLabeled, + BoundingBox2DLabeledWithCategory, + BoundingBoxes2DLabeledStamped, BoundingBoxLabeled, + BoundingBoxLabeledWithCategory, BoundingboxStamped, + Empty, GeodeticCoordinates, Header, Label, + LabelsWithCategory, LabelWithInstance, Point, + PointCloud2, PointField, ProjectCreation, ProjectInfo, + ProjectInfos, Query, QueryInstance, TimeInterval, + Timestamp, TransformStampedQuery, UuidDatatypePair) from seerep.fb import meta_operations_grpc_fb as metaOperations From 2e5278d2b455faf2b1deee3be0d6df85d258732f Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Tue, 11 Apr 2023 15:19:03 +0000 Subject: [PATCH 15/21] datatype fix --- seerep_srv/seerep_server/src/fb_meta_operations.cpp | 4 ++-- seerep_srv/seerep_server/src/pb_meta_operations.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index b4cb9c27a..748555536 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -211,12 +211,12 @@ FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, // isolate second and nano second bits from min time uint64_t mintime = timeinterval.min_corner().get<0>(); uint32_t min_nanos = (uint32_t)mintime; - uint32_t min_seconds = (uint32_t)(mintime >> 32); + uint32_t min_seconds = (int32_t)(mintime >> 32); // isolate second and nano second bits from max time uint64_t maxtime = timeinterval.max_corner().get<0>(); uint32_t max_nanos = (uint32_t)maxtime; - uint32_t max_seconds = (uint32_t)(maxtime >> 32); + uint32_t max_seconds = (int32_t)(maxtime >> 32); flatbuffers::grpc::MessageBuilder builder; diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index ac2fa5dd8..a7803f7cd 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -131,12 +131,12 @@ grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* conte // isolate second and nano second bits from min time uint64_t mintime = timeinterval.min_corner().get<0>(); uint32_t min_nanos = (uint32_t)mintime; - uint32_t min_seconds = (uint32_t)(mintime >> 32); + uint32_t min_seconds = (int32_t)(mintime >> 32); // isolate second and nano second bits from max time uint64_t maxtime = timeinterval.max_corner().get<0>(); uint32_t max_nanos = (uint32_t)maxtime; - uint32_t max_seconds = (uint32_t)(maxtime >> 32); + uint32_t max_seconds = (int32_t)(maxtime >> 32); response->mutable_time_min()->set_nanos(min_nanos); response->mutable_time_min()->set_seconds(min_seconds); From 2b891259690f480077858414b3841204a93a3a52 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Thu, 27 Apr 2023 16:45:37 +0200 Subject: [PATCH 16/21] adjust to new boundingbox format --- .../fbs/uuid_datatype_pair.fbs | 0 .../protos/datatype.proto | 0 .../protos/uuid_datatype_pair.proto | 4 ++- .../seerep_server/pb_meta_operations.h | 8 ++--- .../seerep_server/src/fb_meta_operations.cpp | 34 ++++++++++++------- .../seerep_server/src/pb_meta_operations.cpp | 30 ++++++++++------ 6 files changed, 49 insertions(+), 27 deletions(-) rename {seerep-msgs => seerep_msgs}/fbs/uuid_datatype_pair.fbs (100%) rename {seerep-msgs => seerep_msgs}/protos/datatype.proto (100%) rename {seerep-msgs => seerep_msgs}/protos/uuid_datatype_pair.proto (75%) diff --git a/seerep-msgs/fbs/uuid_datatype_pair.fbs b/seerep_msgs/fbs/uuid_datatype_pair.fbs similarity index 100% rename from seerep-msgs/fbs/uuid_datatype_pair.fbs rename to seerep_msgs/fbs/uuid_datatype_pair.fbs diff --git a/seerep-msgs/protos/datatype.proto b/seerep_msgs/protos/datatype.proto similarity index 100% rename from seerep-msgs/protos/datatype.proto rename to seerep_msgs/protos/datatype.proto diff --git a/seerep-msgs/protos/uuid_datatype_pair.proto b/seerep_msgs/protos/uuid_datatype_pair.proto similarity index 75% rename from seerep-msgs/protos/uuid_datatype_pair.proto rename to seerep_msgs/protos/uuid_datatype_pair.proto index a41dc8221..81e840d63 100644 --- a/seerep-msgs/protos/uuid_datatype_pair.proto +++ b/seerep_msgs/protos/uuid_datatype_pair.proto @@ -1,6 +1,8 @@ syntax = "proto3"; -package seerep; +package seerep.pb; + +import "point.proto"; import "datatype.proto"; diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h index b443d237d..490c995c7 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h @@ -22,10 +22,10 @@ class PbMetaOperations final : public seerep::pb::MetaOperations::Service seerep::pb::ProjectInfo* response); grpc::Status GetProjects(grpc::ServerContext* context, const google::protobuf::Empty* request, seerep::pb::ProjectInfos* response); - grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, - seerep::TimeInterval* response); - grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::UuidDatatypePair* request, - seerep::Boundingbox* response); + grpc::Status GetOverallTimeInterval(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, + seerep::pb::TimeInterval* response); + grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, + seerep::pb::Boundingbox* response); private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 748555536..54ee44a6e 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -306,23 +306,33 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); + // center + int center_x = (overallBB.min_corner().get<0>() + overallBB.max_corner().get<0>()) / 2; + int center_y = (overallBB.min_corner().get<1>() + overallBB.max_corner().get<1>()) / 2; + int center_z = (overallBB.min_corner().get<2>() + overallBB.max_corner().get<2>()) / 2; + + // spatial extent + int se_x = (overallBB.max_corner().get<0>() - overallBB.min_corner().get<0>()); + int se_y = (overallBB.max_corner().get<1>() - overallBB.min_corner().get<1>()); + int se_z = (overallBB.max_corner().get<2>() - overallBB.min_corner().get<2>()); + flatbuffers::grpc::MessageBuilder builder; - seerep::fb::PointBuilder minPointBuilder(builder); - minPointBuilder.add_x(overallBB.min_corner().get<0>()); - minPointBuilder.add_y(overallBB.min_corner().get<1>()); - minPointBuilder.add_z(overallBB.min_corner().get<2>()); - flatbuffers::Offset minPoint = minPointBuilder.Finish(); + seerep::fb::PointBuilder centerPointBuilder(builder); + centerPointBuilder.add_x(center_x); + centerPointBuilder.add_y(center_y); + centerPointBuilder.add_z(center_z); + flatbuffers::Offset centerPoint = centerPointBuilder.Finish(); - seerep::fb::PointBuilder maxPointBuilder(builder); - maxPointBuilder.add_x(overallBB.max_corner().get<0>()); - maxPointBuilder.add_y(overallBB.max_corner().get<1>()); - maxPointBuilder.add_z(overallBB.max_corner().get<2>()); - flatbuffers::Offset maxPoint = maxPointBuilder.Finish(); + seerep::fb::PointBuilder spatialExtentBuilder(builder); + spatialExtentBuilder.add_x(se_x); + spatialExtentBuilder.add_y(se_y); + spatialExtentBuilder.add_z(se_z); + flatbuffers::Offset spatialExtent = spatialExtentBuilder.Finish(); seerep::fb::BoundingboxBuilder boundingBoxBuilder(builder); - boundingBoxBuilder.add_point_min(minPoint); - boundingBoxBuilder.add_point_max(maxPoint); + boundingBoxBuilder.add_center_point(centerPoint); + boundingBoxBuilder.add_spatial_extent(spatialExtent); flatbuffers::Offset bb = boundingBoxBuilder.Finish(); builder.Finish(bb); diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index a7803f7cd..8c0d7dee3 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -95,8 +95,8 @@ grpc::Status PbMetaOperations::GetProjects(grpc::ServerContext* context, const g } grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, - const seerep::UuidDatatypePair* request, - seerep::TimeInterval* response) + const seerep::pb::UuidDatatypePair* request, + seerep::pb::TimeInterval* response) { (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall time interval"; @@ -162,8 +162,8 @@ grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* conte return grpc::Status::OK; } grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, - const seerep::UuidDatatypePair* request, - seerep::Boundingbox* response) + const seerep::pb::UuidDatatypePair* request, + seerep::pb::Boundingbox* response) { (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; @@ -195,15 +195,25 @@ grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* contex seerep_core_msgs::AABB overallBB = seerepCore->getOverallBound(uuidFromString, dt_vector); + // center + int center_x = (overallBB.min_corner().get<0>() + overallBB.max_corner().get<0>()) / 2; + int center_y = (overallBB.min_corner().get<1>() + overallBB.max_corner().get<1>()) / 2; + int center_z = (overallBB.min_corner().get<2>() + overallBB.max_corner().get<2>()) / 2; + + // spatial extent + int se_x = (overallBB.max_corner().get<0>() - overallBB.min_corner().get<0>()); + int se_y = (overallBB.max_corner().get<1>() - overallBB.min_corner().get<1>()); + int se_z = (overallBB.max_corner().get<2>() - overallBB.min_corner().get<2>()); + flatbuffers::grpc::MessageBuilder builder; - response->mutable_point_min()->set_x(overallBB.min_corner().get<0>()); - response->mutable_point_min()->set_y(overallBB.min_corner().get<1>()); - response->mutable_point_min()->set_z(overallBB.min_corner().get<2>()); + response->mutable_center_point()->set_x(center_x); + response->mutable_center_point()->set_y(center_y); + response->mutable_center_point()->set_z(center_z); - response->mutable_point_max()->set_x(overallBB.max_corner().get<0>()); - response->mutable_point_max()->set_y(overallBB.max_corner().get<1>()); - response->mutable_point_max()->set_z(overallBB.max_corner().get<2>()); + response->mutable_spatial_extent()->set_x(se_x); + response->mutable_spatial_extent()->set_y(se_y); + response->mutable_spatial_extent()->set_z(se_z); } catch (const std::exception& e) { From 68100c0da635dc67881ad0050253e40c66c8bb9a Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Thu, 27 Apr 2023 16:47:41 +0200 Subject: [PATCH 17/21] full example --- .isort.cfg | 2 +- ...ndingBox.py => gRPC_fb_getOverallBound.py} | 39 +++++++++++++------ .../gRPC/meta/gRPC_pb_getOverallBound.py | 10 +++-- 3 files changed, 35 insertions(+), 16 deletions(-) rename examples/python/gRPC/meta/{gRPC_fb_getOverallBoundingBox.py => gRPC_fb_getOverallBound.py} (53%) diff --git a/.isort.cfg b/.isort.cfg index 8b92fe5ca..19055d88f 100644 --- a/.isort.cfg +++ b/.isort.cfg @@ -1,3 +1,3 @@ [settings] -known_third_party = cv2,datatype_pb2,fb,flatbuffers,google,grpc,imageio,matplotlib,meta_operations_pb2_grpc,numpy,quaternion,rospy,seerep,sensor_msgs,setuptools,std_msgs,tf,util,util_fb,util_visualize,uuid_datatype_pair_pb2,yaml +known_third_party = cv2,fb,flatbuffers,google,grpc,imageio,matplotlib,numpy,quaternion,rospy,seerep,sensor_msgs,setuptools,std_msgs,tf,util,util_fb,util_visualize,yaml profile =black diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py similarity index 53% rename from examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py rename to examples/python/gRPC/meta/gRPC_fb_getOverallBound.py index b99d36ad4..bd464b5cc 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBoundingBox.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py @@ -4,8 +4,8 @@ import sys import flatbuffers -from fb import Boundingbox, Datatype -from fb import meta_operations_grpc_fb as metaOperations +from seerep.fb import Boundingbox, Datatype, TimeInterval +from seerep.fb import meta_operations_grpc_fb as metaOperations # importing util functions. Assuming that these files are in the parent dir # examples/python/gRPC/util.py @@ -22,7 +22,7 @@ # 1. Get all projects from the server -projectuuid = util_fb.getProject(builder, channel, 'LabeledImagesInGrid') +projectuuid = util_fb.getProject(builder, channel, 'testproject') # 2. Check if the defined project exist; if not exit if not projectuuid: @@ -41,18 +41,35 @@ response = Boundingbox.Boundingbox.GetRootAs(responseBuf) print( - "Min Point (X, Y, Z): " - + str(response.PointMin().X()) + "Center Point (X, Y, Z): " + + str(response.CenterPoint().X()) + " , " - + str(response.PointMin().Y()) + + str(response.CenterPoint().Y()) + " , " - + str(response.PointMin().Z()) + + str(response.CenterPoint().Z()) ) + print( - "Max Point (X, Y, Z): " - + str(response.PointMax().X()) + "Spatial Things Point (X, Y, Z): " + + str(response.SpatialExtent().X()) + " , " - + str(response.PointMax().Y()) + + str(response.SpatialExtent().Y()) + " , " - + str(response.PointMax().Z()) + + str(response.SpatialExtent().Z()) +) + +responseBuf = stub.GetOverallTimeInterval(bytes(buf)) +response = TimeInterval.TimeInterval.GetRootAs(responseBuf) + +print( + " Minimum Time " + + str(response.TimeMin().Seconds()) + + "s and " + + str(response.TimeMin().Nanos()) + + "ms\n" + + " Maximum Time " + + str(response.TimeMax().Seconds()) + + "s and " + + str(response.TimeMax().Nanos()) + + "ms" ) diff --git a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py index f99901e15..7e9925467 100755 --- a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py +++ b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py @@ -3,9 +3,9 @@ import os import sys -import datatype_pb2 as datatype -import meta_operations_pb2_grpc as metaOperations -import uuid_datatype_pair_pb2 as uuid_datatype_pair +import seerep.pb.datatype_pb2 as datatype +import seerep.pb.meta_operations_pb2_grpc as metaOperations +import seerep.pb.uuid_datatype_pair_pb2 as uuid_datatype_pair from google.protobuf import empty_pb2 script_dir = os.path.dirname(__file__) @@ -23,7 +23,7 @@ projectuuid = "" for project in response.projects: print(project.name + " " + project.uuid + "\n") - if project.name == "LabeledImagesInGrid": + if project.name == "testproject": projectuuid = project.uuid uuiddt = uuid_datatype_pair.UuidDatatypePair() @@ -31,5 +31,7 @@ uuiddt.datatype = datatype.image response = stub.GetOverallTimeInterval(uuiddt) +print(response) +response = stub.GetOverallBoundingBox(uuiddt) print(response) From 9e78d950493f5b2b1159b13607deed26573d6b86 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Thu, 27 Apr 2023 16:55:08 +0200 Subject: [PATCH 18/21] application of isort --- .isort.cfg | 2 +- .../python/ROS/ROS_publishSimplePointcloud.py | 1 + .../gRPC/images/gRPC_pb_sendLabeledImage.py | 12 ++--- .../images/gRPC_pb_sendLabeledImageGrid.py | 12 ++--- .../gRPC/meta/gRPC_fb_getOverallTime.py | 44 ------------------- ...gRPC_fb_sendPointsBasedOnImageInstances.py | 14 +++++- .../gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 22 +++++++--- .../loadSimulatedDataWithInstancePosition.py | 22 +++++++--- examples/python/gRPC/util_fb.py | 36 +++++++++++---- 9 files changed, 85 insertions(+), 80 deletions(-) delete mode 100755 examples/python/gRPC/meta/gRPC_fb_getOverallTime.py diff --git a/.isort.cfg b/.isort.cfg index 19055d88f..b4d08537e 100644 --- a/.isort.cfg +++ b/.isort.cfg @@ -1,3 +1,3 @@ [settings] -known_third_party = cv2,fb,flatbuffers,google,grpc,imageio,matplotlib,numpy,quaternion,rospy,seerep,sensor_msgs,setuptools,std_msgs,tf,util,util_fb,util_visualize,yaml +known_third_party = cv2,flatbuffers,google,grpc,imageio,matplotlib,numpy,quaternion,rospy,seerep,sensor_msgs,setuptools,std_msgs,tf,util,util_fb,util_visualize,yaml profile =black diff --git a/examples/python/ROS/ROS_publishSimplePointcloud.py b/examples/python/ROS/ROS_publishSimplePointcloud.py index 21465758d..f2ccf9d37 100755 --- a/examples/python/ROS/ROS_publishSimplePointcloud.py +++ b/examples/python/ROS/ROS_publishSimplePointcloud.py @@ -7,6 +7,7 @@ import rospy from sensor_msgs import point_cloud2 + # PointCloud2 color cube # https://answers.ros.org/question/289576/understanding-the-bytes-in-a-pcl2-message/ from sensor_msgs.msg import PointCloud2, PointField diff --git a/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py b/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py index 3e2f570f0..1c1e6206e 100755 --- a/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py +++ b/examples/python/gRPC/images/gRPC_pb_sendLabeledImage.py @@ -8,15 +8,15 @@ import numpy as np from google.protobuf import empty_pb2 from seerep.pb import boundingbox2d_labeled_pb2 as boundingbox2d_labeled -from seerep.pb import \ - boundingbox2d_labeled_with_category_pb2 as \ - boundingbox2d_labeled_with_category +from seerep.pb import ( + boundingbox2d_labeled_with_category_pb2 as boundingbox2d_labeled_with_category, +) from seerep.pb import image_pb2 as image from seerep.pb import image_service_pb2_grpc as imageService from seerep.pb import label_with_instance_pb2 as labelWithInstance -from seerep.pb import \ - labels_with_instance_with_category_pb2 as \ - labels_with_instance_with_category +from seerep.pb import ( + labels_with_instance_with_category_pb2 as labels_with_instance_with_category, +) from seerep.pb import meta_operations_pb2_grpc as metaOperations from seerep.pb import projectCreation_pb2 as projectCreation from seerep.pb import tf_service_pb2_grpc as tfService diff --git a/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py b/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py index 8b204e1e6..5f546b770 100755 --- a/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py +++ b/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py @@ -7,15 +7,15 @@ import numpy as np from seerep.pb import boundingbox2d_labeled_pb2 as boundingbox2d_labeled -from seerep.pb import \ - boundingbox2d_labeled_with_category_pb2 as \ - boundingbox2d_labeled_with_category +from seerep.pb import ( + boundingbox2d_labeled_with_category_pb2 as boundingbox2d_labeled_with_category, +) from seerep.pb import image_pb2 as image from seerep.pb import image_service_pb2_grpc as imageService from seerep.pb import label_with_instance_pb2 as labelWithInstance -from seerep.pb import \ - labels_with_instance_with_category_pb2 as \ - labels_with_instance_with_category +from seerep.pb import ( + labels_with_instance_with_category_pb2 as labels_with_instance_with_category, +) from seerep.pb import meta_operations_pb2_grpc as metaOperations from seerep.pb import projectCreation_pb2 as projectCreation from seerep.pb import tf_service_pb2_grpc as tfService diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py b/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py deleted file mode 100755 index 8927fc2a0..000000000 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallTime.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python3 - -import os -import sys - -import flatbuffers -from fb import Datatype, TimeInterval -from fb import meta_operations_grpc_fb as metaOperations - -# importing util functions. Assuming that these files are in the parent dir -# examples/python/gRPC/util.py -# examples/python/gRPC/util_fb.py -script_dir = os.path.dirname(__file__) -util_dir = os.path.join(script_dir, '..') -sys.path.append(util_dir) -import util -import util_fb - -builder = flatbuffers.Builder(1024) -# Default server is localhost ! -channel = util.get_gRPC_channel() - - -# 1. Get all projects from the server -projectuuid = util_fb.getProject(builder, channel, 'LabeledImagesInGrid') - -# 2. Check if the defined project exist; if not exit -if not projectuuid: - print("Project Not Found") - exit() - -# 3. Get gRPC service object -stub = metaOperations.MetaOperationsStub(channel) - -UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, Datatype.Datatype().Image) - -builder.Finish(UuidDatatypePair) -buf = builder.Output() - -responseBuf = stub.GetOverallTimeInterval(bytes(buf)) -response = TimeInterval.TimeInterval.GetRootAs(responseBuf) - -print("time min (sec/nanos): " + str(response.TimeMin().Seconds()) + " / " + str(response.TimeMin().Nanos())) -print("time max (sec/nanos): " + str(response.TimeMax().Seconds()) + " / " + str(response.TimeMax().Nanos())) diff --git a/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py b/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py index c14d1e2e4..85ca64fa5 100755 --- a/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py +++ b/examples/python/gRPC/point/gRPC_fb_sendPointsBasedOnImageInstances.py @@ -4,8 +4,18 @@ import sys import flatbuffers -from seerep.fb import (Datatypes, Header, Image, Integer, LabelWithInstance, - Point, PointStamped, String, Timestamp, UnionMapEntry) +from seerep.fb import ( + Datatypes, + Header, + Image, + Integer, + LabelWithInstance, + Point, + PointStamped, + String, + Timestamp, + UnionMapEntry, +) from seerep.fb import image_service_grpc_fb as imageService from seerep.fb import point_service_grpc_fb as pointService diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index f874d676e..02d947a83 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -17,13 +17,21 @@ sys.path.append(util_dir) import util -from util_fb import (addToBoundingBoxLabeledVector, addToGeneralLabelsVector, - addToPointFieldVector, createBoundingBoxes, - createBoundingBoxesLabeled, - createBoundingBoxLabeledWithCategory, createHeader, - createLabelsWithInstance, createLabelWithCategory, - createPoint, createPointFields, createTimeStamp, - getOrCreateProject) +from util_fb import ( + addToBoundingBoxLabeledVector, + addToGeneralLabelsVector, + addToPointFieldVector, + createBoundingBoxes, + createBoundingBoxesLabeled, + createBoundingBoxLabeledWithCategory, + createHeader, + createLabelsWithInstance, + createLabelWithCategory, + createPoint, + createPointFields, + createTimeStamp, + getOrCreateProject, +) NUM_GENERAL_LABELS = 1 NUM_BB_LABELS = 1 diff --git a/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py b/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py index 0b480ee5a..33a321bbe 100755 --- a/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py +++ b/examples/python/gRPC/simulated-data/loadSimulatedDataWithInstancePosition.py @@ -8,11 +8,23 @@ import numpy as np import quaternion import yaml -from seerep.fb import (Boundingbox2D, BoundingBox2DLabeled, - BoundingBox2DLabeledWithCategory, Header, Image, - LabelsWithCategory, LabelWithInstance, Point, Point2D, - PointStamped, Quaternion, Timestamp, Transform, - TransformStamped, Vector3) +from seerep.fb import ( + Boundingbox2D, + BoundingBox2DLabeled, + BoundingBox2DLabeledWithCategory, + Header, + Image, + LabelsWithCategory, + LabelWithInstance, + Point, + Point2D, + PointStamped, + Quaternion, + Timestamp, + Transform, + TransformStamped, + Vector3, +) from seerep.fb import image_service_grpc_fb as imageService from seerep.fb import point_service_grpc_fb as pointService from seerep.fb import tf_service_grpc_fb as tfService diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index da37bdf06..81489ee0a 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -1,14 +1,32 @@ import sys -from seerep.fb import (Boundingbox, BoundingBox2DLabeled, - BoundingBox2DLabeledWithCategory, - BoundingBoxes2DLabeledStamped, BoundingBoxLabeled, - BoundingBoxLabeledWithCategory, BoundingboxStamped, - Empty, GeodeticCoordinates, Header, Label, - LabelsWithCategory, LabelWithInstance, Point, - PointCloud2, PointField, ProjectCreation, ProjectInfo, - ProjectInfos, Query, QueryInstance, TimeInterval, - Timestamp, TransformStampedQuery, UuidDatatypePair) +from seerep.fb import ( + Boundingbox, + BoundingBox2DLabeled, + BoundingBox2DLabeledWithCategory, + BoundingBoxes2DLabeledStamped, + BoundingBoxLabeled, + BoundingBoxLabeledWithCategory, + BoundingboxStamped, + Empty, + GeodeticCoordinates, + Header, + Label, + LabelsWithCategory, + LabelWithInstance, + Point, + PointCloud2, + PointField, + ProjectCreation, + ProjectInfo, + ProjectInfos, + Query, + QueryInstance, + TimeInterval, + Timestamp, + TransformStampedQuery, + UuidDatatypePair, +) from seerep.fb import meta_operations_grpc_fb as metaOperations From 0889e3e01df44012005e52d7fbc38e3f50f7ac97 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 3 May 2023 13:56:29 +0200 Subject: [PATCH 19/21] get all categories and labels --- .../gRPC/meta/gRPC_fb_getOverallBound.py | 35 +++- .../gRPC/meta/gRPC_pb_getOverallBound.py | 12 ++ examples/python/gRPC/util_fb.py | 12 ++ seerep_com/fbs/meta_operations.fbs | 5 + seerep_com/protos/meta_operations.proto | 5 + seerep_msgs/CMakeLists.txt | 6 + seerep_msgs/fbs/categories.fbs | 6 + seerep_msgs/fbs/labels.fbs | 6 + .../fbs/uuid_datatype_with_category.fbs | 9 + seerep_msgs/protos/categories.proto | 8 + seerep_msgs/protos/labels.proto | 8 + seerep_msgs/protos/uuid_datatype_pair.proto | 2 - .../protos/uuid_datatype_with_category.proto | 11 ++ .../seerep_core/include/seerep_core/core.h | 17 ++ .../include/seerep_core/core_dataset.h | 16 ++ .../include/seerep_core/core_project.h | 9 + seerep_srv/seerep_core/src/core.cpp | 14 ++ seerep_srv/seerep_core/src/core_dataset.cpp | 58 ++++++ seerep_srv/seerep_core/src/core_project.cpp | 11 ++ .../seerep_server/fb_meta_operations.h | 6 + .../seerep_server/pb_meta_operations.h | 4 + .../seerep_server/src/fb_meta_operations.cpp | 172 ++++++++++++++++++ .../seerep_server/src/pb_meta_operations.cpp | 116 ++++++++++++ 23 files changed, 545 insertions(+), 3 deletions(-) create mode 100644 seerep_msgs/fbs/categories.fbs create mode 100644 seerep_msgs/fbs/labels.fbs create mode 100644 seerep_msgs/fbs/uuid_datatype_with_category.fbs create mode 100644 seerep_msgs/protos/categories.proto create mode 100644 seerep_msgs/protos/labels.proto create mode 100644 seerep_msgs/protos/uuid_datatype_with_category.proto diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py index bd464b5cc..faafbb60f 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py @@ -4,7 +4,7 @@ import sys import flatbuffers -from seerep.fb import Boundingbox, Datatype, TimeInterval +from seerep.fb import Boundingbox, Categories, Datatype, Labels, TimeInterval from seerep.fb import meta_operations_grpc_fb as metaOperations # importing util functions. Assuming that these files are in the parent dir @@ -37,6 +37,9 @@ builder.Finish(UuidDatatypePair) buf = builder.Output() +### +# Fetching overall spatial bound + responseBuf = stub.GetOverallBoundingBox(bytes(buf)) response = Boundingbox.Boundingbox.GetRootAs(responseBuf) @@ -58,6 +61,9 @@ + str(response.SpatialExtent().Z()) ) +### +# Fetching overall temporal bound + responseBuf = stub.GetOverallTimeInterval(bytes(buf)) response = TimeInterval.TimeInterval.GetRootAs(responseBuf) @@ -73,3 +79,30 @@ + str(response.TimeMax().Nanos()) + "ms" ) + +### +# Fetching all category names + +responseBuf = stub.GetAllCategories(bytes(buf)) +response = Categories.Categories.GetRootAs(responseBuf) + +print("Saved Category names are:") +for idx in range(response.CategoriesLength()): + print(response.Categories(idx).decode()) + +### +# Fetching all label names for a given category + +builder = flatbuffers.Builder(1024) + +UuidDatatypeWithCategory = util_fb.createUuidDatatypeWithCategory(builder, projectuuid, Datatype.Datatype().Image, "3") + +builder.Finish(UuidDatatypeWithCategory) +buf = builder.Output() + +responseBuf = stub.GetAllLabels(bytes(buf)) +response = Labels.Labels.GetRootAs(responseBuf) + +print("Saved Label names are:") +for idx in range(response.LabelsLength()): + print(response.Labels(idx).decode()) diff --git a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py index 7e9925467..9cee0b050 100755 --- a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py +++ b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py @@ -6,6 +6,7 @@ import seerep.pb.datatype_pb2 as datatype import seerep.pb.meta_operations_pb2_grpc as metaOperations import seerep.pb.uuid_datatype_pair_pb2 as uuid_datatype_pair +import seerep.pb.uuid_datatype_with_category_pb2 as uuid_datatype_with_category from google.protobuf import empty_pb2 script_dir = os.path.dirname(__file__) @@ -35,3 +36,14 @@ response = stub.GetOverallBoundingBox(uuiddt) print(response) + +response = stub.GetAllCategories(uuiddt) +print(response) + +uuiddt_category = uuid_datatype_with_category.UuidDatatypeWithCategory() +uuiddt_category.category = "3" +uuiddt_category.uuid_with_datatype.projectuuid = projectuuid +uuiddt_category.uuid_with_datatype.datatype = datatype.image + +response = stub.GetAllLabels(uuiddt_category) +print(response) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 81489ee0a..24ca88510 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -26,6 +26,7 @@ Timestamp, TransformStampedQuery, UuidDatatypePair, + UuidDatatypeWithCategory, ) from seerep.fb import meta_operations_grpc_fb as metaOperations @@ -458,6 +459,17 @@ def createUuidDatatypePair(builder, uuid, datatype): return UuidDatatypePair.End(builder) +def createUuidDatatypeWithCategory(builder, uuid, datatype, category): + categoryStr = builder.CreateString(category) + + UuidDatatypePair = createUuidDatatypePair(builder, uuid, datatype) + + UuidDatatypeWithCategory.Start(builder) + UuidDatatypeWithCategory.AddCategory(builder, categoryStr) + UuidDatatypeWithCategory.AddUuidAndDatatype(builder, UuidDatatypePair) + return UuidDatatypeWithCategory.End(builder) + + def createProjectInfo(builder, name, uuid): nameStr = builder.CreateString(name) uuidStr = builder.CreateString(uuid) diff --git a/seerep_com/fbs/meta_operations.fbs b/seerep_com/fbs/meta_operations.fbs index 51630e18f..7246f7c8b 100644 --- a/seerep_com/fbs/meta_operations.fbs +++ b/seerep_com/fbs/meta_operations.fbs @@ -6,6 +6,9 @@ include "empty.fbs"; include "boundingbox.fbs"; include "time_interval.fbs"; include "uuid_datatype_pair.fbs"; +include "uuid_datatype_with_category.fbs"; +include "labels.fbs"; +include "categories.fbs"; namespace seerep.fb; @@ -16,4 +19,6 @@ rpc_service MetaOperations { DeleteProject(seerep.fb.ProjectInfo):seerep.fb.Empty; GetOverallTimeInterval(seerep.fb.UuidDatatypePair):seerep.fb.TimeInterval; GetOverallBoundingBox(seerep.fb.UuidDatatypePair):seerep.fb.Boundingbox; + GetAllCategories(seerep.fb.UuidDatatypePair):seerep.fb.Categories; + GetAllLabels(seerep.fb.UuidDatatypeWithCategory):seerep.fb.Labels; } diff --git a/seerep_com/protos/meta_operations.proto b/seerep_com/protos/meta_operations.proto index 6db06249b..14946646c 100644 --- a/seerep_com/protos/meta_operations.proto +++ b/seerep_com/protos/meta_operations.proto @@ -10,6 +10,9 @@ import "projectCreation.proto"; import "uuid_datatype_pair.proto"; import "time_interval.proto"; import "boundingbox.proto"; +import "labels.proto"; +import "categories.proto"; +import "uuid_datatype_with_category.proto"; service MetaOperations { @@ -17,4 +20,6 @@ service MetaOperations rpc GetProjects(google.protobuf.Empty) returns (ProjectInfos); rpc GetOverallTimeInterval(UuidDatatypePair) returns (TimeInterval); rpc GetOverallBoundingBox(UuidDatatypePair) returns (Boundingbox); + rpc GetAllCategories(UuidDatatypePair) returns (Categories); + rpc GetAllLabels(UuidDatatypeWithCategory) returns (Labels); } diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 5b992e9a1..d606a60e5 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(MY_PROTO_FILES protos/boundingbox2d_labeled_with_category.proto protos/boundingbox2d_stamped.proto protos/boundingbox2d.proto + protos/categories.proto protos/datatype.proto protos/frame_infos.proto protos/frame_query.proto @@ -31,6 +32,7 @@ set(MY_PROTO_FILES protos/image.proto protos/label_with_instance.proto protos/label.proto + protos/labels.proto protos/labels_with_category.proto protos/labels_with_instance_with_category.proto protos/point_cloud_2.proto @@ -58,6 +60,7 @@ set(MY_PROTO_FILES protos/twist_with_covariance_stamped.proto protos/twist_with_covariance.proto protos/twist.proto + protos/uuid_datatype_with_category.proto protos/uuid_datatype_pair.proto protos/vector3_stamped.proto protos/vector3.proto @@ -74,6 +77,7 @@ set(MY_FBS_FILES fbs/boundingbox2d_stamped.fbs fbs/boundingbox2d.fbs fbs/boundingboxes2d_labeled_stamped.fbs + fbs/categories.fbs fbs/datatype.fbs fbs/empty.fbs fbs/frame_infos.fbs @@ -83,6 +87,7 @@ set(MY_FBS_FILES fbs/image.fbs fbs/label_with_instance.fbs fbs/label.fbs + fbs/labels.fbs fbs/labels_with_category.fbs fbs/labels_with_instance_with_category.fbs fbs/point_cloud_2.fbs @@ -114,6 +119,7 @@ set(MY_FBS_FILES fbs/union_map_entry.fbs fbs/uuids_per_project.fbs fbs/uuids_project_pair.fbs + fbs/uuid_datatype_with_category.fbs fbs/uuid_datatype_pair.fbs fbs/vector3_stamped.fbs fbs/vector3.fbs diff --git a/seerep_msgs/fbs/categories.fbs b/seerep_msgs/fbs/categories.fbs new file mode 100644 index 000000000..917ae8866 --- /dev/null +++ b/seerep_msgs/fbs/categories.fbs @@ -0,0 +1,6 @@ + +namespace seerep.fb; + +table Categories { + categories:[string]; +} diff --git a/seerep_msgs/fbs/labels.fbs b/seerep_msgs/fbs/labels.fbs new file mode 100644 index 000000000..0bda02c3e --- /dev/null +++ b/seerep_msgs/fbs/labels.fbs @@ -0,0 +1,6 @@ + +namespace seerep.fb; + +table Labels { + labels:[string]; +} diff --git a/seerep_msgs/fbs/uuid_datatype_with_category.fbs b/seerep_msgs/fbs/uuid_datatype_with_category.fbs new file mode 100644 index 000000000..3035b1907 --- /dev/null +++ b/seerep_msgs/fbs/uuid_datatype_with_category.fbs @@ -0,0 +1,9 @@ + +include "uuid_datatype_pair.fbs"; + +namespace seerep.fb; + +table UuidDatatypeWithCategory { + category:string; + UuidAndDatatype:UuidDatatypePair; +} diff --git a/seerep_msgs/protos/categories.proto b/seerep_msgs/protos/categories.proto new file mode 100644 index 000000000..1c6dfe0f7 --- /dev/null +++ b/seerep_msgs/protos/categories.proto @@ -0,0 +1,8 @@ +syntax = "proto3"; + +package seerep.pb; + +message Categories +{ + repeated string categories = 1; +} diff --git a/seerep_msgs/protos/labels.proto b/seerep_msgs/protos/labels.proto new file mode 100644 index 000000000..70f5c584f --- /dev/null +++ b/seerep_msgs/protos/labels.proto @@ -0,0 +1,8 @@ +syntax = "proto3"; + +package seerep.pb; + +message Labels +{ + repeated string labels = 1; +} diff --git a/seerep_msgs/protos/uuid_datatype_pair.proto b/seerep_msgs/protos/uuid_datatype_pair.proto index 81e840d63..bd9303571 100644 --- a/seerep_msgs/protos/uuid_datatype_pair.proto +++ b/seerep_msgs/protos/uuid_datatype_pair.proto @@ -2,8 +2,6 @@ syntax = "proto3"; package seerep.pb; -import "point.proto"; - import "datatype.proto"; message UuidDatatypePair diff --git a/seerep_msgs/protos/uuid_datatype_with_category.proto b/seerep_msgs/protos/uuid_datatype_with_category.proto new file mode 100644 index 000000000..1b6f997ae --- /dev/null +++ b/seerep_msgs/protos/uuid_datatype_with_category.proto @@ -0,0 +1,11 @@ +syntax = "proto3"; + +package seerep.pb; + +import "uuid_datatype_pair.proto"; + +message UuidDatatypeWithCategory +{ + string category = 1; + UuidDatatypePair uuid_with_datatype = 2; +} diff --git a/seerep_srv/seerep_core/include/seerep_core/core.h b/seerep_srv/seerep_core/include/seerep_core/core.h index 2999169f9..e9f3d06c2 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core.h +++ b/seerep_srv/seerep_core/include/seerep_core/core.h @@ -148,6 +148,23 @@ class Core */ seerep_core_msgs::AABB getOverallBound(boost::uuids::uuid uuid, std::vector datatypes); + /** + * @brief Get the all categories saved in a project + * + * @param uuid of project + * @param datatypes A vector of datatypes + * @return std::vector vectir if categories + */ + std::vector getAllCategories(boost::uuids::uuid uuid, std::vector datatypes); + + /** + * @brief Get the all labels saved in a project + * + * @return std::vector uuid of project + */ + std::vector getAllLabels(boost::uuids::uuid uuid, std::vector datatypes, + std::string category); + private: /** * @brief Returns an iterator to the project with the given uuid. Throws an error if not found diff --git a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h index 0e91170da..f7b64f1e1 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -1,6 +1,7 @@ #ifndef SEEREP_CORE_CORE_DATASET_H_ #define SEEREP_CORE_CORE_DATASET_H_ +#include #include #include #include @@ -133,6 +134,21 @@ class CoreDataset */ seerep_core_msgs::AABB getSpatialBounds(std::vector datatypes); + /** + * @brief Get the all categories saved in a project + * + * @param datatypes A vector of datatypes for which the categories have to be fetched + * @return std::vector vector of categories + */ + std::vector getAllCategories(std::vector datatypes); + + /** + * @brief Get the all labels saved in a project + * + * @return std::vector vector of labels + */ + std::vector getAllLabels(std::vector datatypes, std::string category); + private: /** * @brief fills the member variables based on the HDF5 file 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 191016ddc..401e904ed 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_project.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_project.h @@ -145,6 +145,15 @@ class CoreProject seerep_core_msgs::AabbTime getTimeBounds(std::vector datatypes); seerep_core_msgs::AABB getSpatialBounds(std::vector datatypes); + std::vector getAllCategories(std::vector datatypes); + + /** + * @brief Get the all labels saved in a project + * + * @return std::vector vector of labels + */ + std::vector getAllLabels(std::vector datatypes, std::string category); + private: /** * @brief Create the HDF5 file accessor and the mutex. Based on that create the diff --git a/seerep_srv/seerep_core/src/core.cpp b/seerep_srv/seerep_core/src/core.cpp index 667d3772b..e7d37dbcc 100644 --- a/seerep_srv/seerep_core/src/core.cpp +++ b/seerep_srv/seerep_core/src/core.cpp @@ -287,4 +287,18 @@ seerep_core_msgs::AABB Core::getOverallBound(boost::uuids::uuid uuid, std::vecto return project->second->getSpatialBounds(datatypes); } +std::vector Core::getAllCategories(boost::uuids::uuid uuid, + std::vector datatypes) +{ + auto project = findProject(uuid); + return project->second->getAllCategories(datatypes); +} + +std::vector Core::getAllLabels(boost::uuids::uuid uuid, std::vector datatypes, + std::string category) +{ + auto project = findProject(uuid); + return project->second->getAllLabels(datatypes, category); +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index 1f1c4fc45..dae76428e 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -601,4 +601,62 @@ seerep_core_msgs::AABB CoreDataset::getSpatialBounds(std::vector CoreDataset::getAllCategories(std::vector datatypes) +{ + std::vector categories; + + // traverse all datatypes + for (seerep_core_msgs::Datatype dt : datatypes) + { + // obtain the map which holds the categories + auto categories_map = m_datatypeDatatypeSpecificsMap.at(dt)->categoryLabelDatasetsMap; + + // traverse this map + for (auto& [key, value] : categories_map) + { + // obtain the key of this map + std::string category = key; + + // verify this key (label) is not already added + if (std::find(categories.begin(), categories.end(), category) == categories.end()) + { + // if not, add it + categories.push_back(category); + } + } + } + // return the prepared vector + return categories; +} + +std::vector CoreDataset::getAllLabels(std::vector datatypes, + std::string category) +{ + std::vector labels; + + // traverse all datatypes + for (seerep_core_msgs::Datatype dt : datatypes) + { + // obtain the map pertaining to the provided label + std::unordered_map> label_to_uuid_map = + m_datatypeDatatypeSpecificsMap.at(dt)->categoryLabelDatasetsMap[category]; + + // traverse this map + for (auto& [key, value] : label_to_uuid_map) + { + // obtain the key of this map + std::string label = key; + + // verify this key (label) is not already added + if (std::find(labels.begin(), labels.end(), label) == labels.end()) + { + // if not, add it + labels.push_back(label); + } + } + } + // return the prepared vector + return labels; +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index f580da8a7..658f5deb3 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -147,4 +147,15 @@ seerep_core_msgs::AABB CoreProject::getSpatialBounds(std::vectorgetSpatialBounds(datatypes); } +std::vector CoreProject::getAllCategories(std::vector datatypes) +{ + return m_coreDatasets->getAllCategories(datatypes); +} + +std::vector CoreProject::getAllLabels(std::vector datatypes, + std::string category) +{ + return m_coreDatasets->getAllLabels(datatypes, category); +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 9d502bab6..fa6c33d64 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -31,6 +31,12 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response) override; + grpc::Status GetAllCategories(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) override; + grpc::Status GetAllLabels(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) override; private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h index 490c995c7..f1e6b591f 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h @@ -26,6 +26,10 @@ class PbMetaOperations final : public seerep::pb::MetaOperations::Service seerep::pb::TimeInterval* response); grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, seerep::pb::Boundingbox* response); + grpc::Status GetAllCategories(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, + seerep::pb::Categories* response) override; + grpc::Status GetAllLabels(grpc::ServerContext* context, const seerep::pb::UuidDatatypeWithCategory* request, + seerep::pb::Labels* response); private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 54ee44a6e..84be19a7d 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -363,4 +363,176 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, return grpc::Status::OK; } +grpc::Status FbMetaOperations::GetAllCategories(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) +{ + (void)context; // ignore that variable without causing warnings + auto requestRoot = request->GetRoot(); + + // It was not possible to send a vector of enum of datatypes + // through to this function. Therefore, on the flatbuffer service + // level, the UuidDatatypePair implementation has one datatype, + // while below this layer, in the seerep core, the implementation + // is a vector. We intend to make them consistent in the future + // by implementing a vector of enums on the flatbuffer (and protobuf) + // levels. + std::vector dt_vector; + + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatype()); + if (casted_datatype == seerep::fb::Datatype_Image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (casted_datatype == seerep::fb::Datatype_PointCloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (casted_datatype == seerep::fb::Datatype_Point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + try + { + std::string uuid = requestRoot->projectuuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector categories = seerepCore->getAllCategories(uuidFromString, dt_vector); + + flatbuffers::grpc::MessageBuilder builder; + + std::vector> fb_categories; + + for (std::string cat : categories) + { + flatbuffers::Offset fb_category = builder.CreateString(cat); + fb_categories.push_back(fb_category); + } + + auto fb_categories_vector = builder.CreateVector(fb_categories); + + seerep::fb::CategoriesBuilder cb(builder); + cb.add_categories(fb_categories_vector); + builder.Finish(cb.Finish()); + + *response = builder.ReleaseMessage(); + } + catch (std::runtime_error const& e) + { + // mainly catching "invalid uuid string" when transforming uuid_project from string to uuid + // also catching core doesn't have project with uuid error + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } + + return grpc::Status::OK; +} + +grpc::Status +FbMetaOperations::GetAllLabels(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + flatbuffers::grpc::Message* response) +{ + (void)context; // ignore that variable without causing warnings + auto requestRoot = request->GetRoot(); + + // It was not possible to send a vector of enum of datatypes + // through to this function. Therefore, on the flatbuffer service + // level, the UuidDatatypePair implementation has one datatype, + // while below this layer, in the seerep core, the implementation + // is a vector. We intend to make them consistent in the future + // by implementing a vector of enums on the flatbuffer (and protobuf) + // levels. + std::vector dt_vector; + + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->UuidAndDatatype()->datatype()); + if (casted_datatype == seerep::fb::Datatype_Image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (casted_datatype == seerep::fb::Datatype_PointCloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (casted_datatype == seerep::fb::Datatype_Point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + try + { + std::string category = requestRoot->category()->str(); + + std::string uuid = requestRoot->UuidAndDatatype()->projectuuid()->str(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector allLabels = seerepCore->getAllLabels(uuidFromString, dt_vector, category); + + flatbuffers::grpc::MessageBuilder builder; + + std::vector> fb_labels; + + for (std::string lbl : allLabels) + { + flatbuffers::Offset fb_label = builder.CreateString(lbl); + fb_labels.push_back(fb_label); + } + + auto fb_labels_vector = builder.CreateVector(fb_labels); + + seerep::fb::LabelsBuilder lb(builder); + lb.add_labels(fb_labels_vector); + builder.Finish(lb.Finish()); + + *response = builder.ReleaseMessage(); + } + catch (std::runtime_error const& e) + { + // mainly catching "invalid uuid string" when transforming uuid_project from string to uuid + // also catching core doesn't have project with uuid error + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } + return grpc::Status::OK; +} + } /* namespace seerep_server */ diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 8c0d7dee3..38ddee0b4 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -233,4 +233,120 @@ grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* contex return grpc::Status::OK; } +grpc::Status PbMetaOperations::GetAllCategories(grpc::ServerContext* context, + const seerep::pb::UuidDatatypePair* request, + seerep::pb::Categories* response) +{ + (void)context; // ignore that variable without causing warnings + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching all categories"; + + std::vector dt_vector; + + if (request->datatype() == seerep::datatype::image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (request->datatype() == seerep::datatype::pointcloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (request->datatype() == seerep::datatype::point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + try + { + std::string uuid = request->projectuuid(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector categories = seerepCore->getAllCategories(uuidFromString, dt_vector); + + for (std::string category : categories) + { + response->add_categories(category); + } + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } + + return grpc::Status::OK; +} + +grpc::Status PbMetaOperations::GetAllLabels(grpc::ServerContext* context, + const seerep::pb::UuidDatatypeWithCategory* request, + seerep::pb::Labels* response) +{ + (void)context; // ignore that variable without causing warnings + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; + + std::vector dt_vector; + + if (request->uuid_with_datatype().datatype() == seerep::datatype::image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (request->uuid_with_datatype().datatype() == seerep::datatype::pointcloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (request->uuid_with_datatype().datatype() == seerep::datatype::point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + try + { + std::string category = request->category(); + + std::string uuid = request->uuid_with_datatype().projectuuid(); + boost::uuids::string_generator gen; + auto uuidFromString = gen(uuid); + + std::vector allLabels = seerepCore->getAllLabels(uuidFromString, dt_vector, category); + + for (std::string label : allLabels) + { + response->add_labels(label); + } + } + catch (const std::exception& e) + { + // specific handling for all exceptions extending std::exception, except + // std::runtime_error which is handled explicitly + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + catch (...) + { + // catch any other errors (that we have no information about) + std::string msg = "Unknown failure occurred. Possible memory corruption"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << msg; + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } + + return grpc::Status::OK; +} + } /* namespace seerep_server */ From 546fff9730f3b351506f9f7bec2084ea8debc5e9 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 3 May 2023 17:35:32 +0200 Subject: [PATCH 20/21] implementation of datatype all --- .../gRPC/meta/gRPC_fb_getOverallBound.py | 2 +- .../gRPC/meta/gRPC_pb_getOverallBound.py | 2 +- seerep_msgs/core/datatype.h | 3 +- seerep_msgs/fbs/datatype.fbs | 3 +- seerep_msgs/protos/datatype.proto | 1 + .../seerep_server/fb_meta_operations.h | 2 + .../seerep_server/pb_meta_operations.h | 2 + .../seerep_server/src/fb_meta_operations.cpp | 101 +++++++---------- .../seerep_server/src/pb_meta_operations.cpp | 102 ++++++------------ 9 files changed, 82 insertions(+), 136 deletions(-) diff --git a/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py b/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py index faafbb60f..677cfc652 100755 --- a/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py +++ b/examples/python/gRPC/meta/gRPC_fb_getOverallBound.py @@ -32,7 +32,7 @@ # 3. Get gRPC service object stub = metaOperations.MetaOperationsStub(channel) -UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, Datatype.Datatype().Image) +UuidDatatypePair = util_fb.createUuidDatatypePair(builder, projectuuid, Datatype.Datatype().All) builder.Finish(UuidDatatypePair) buf = builder.Output() diff --git a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py index 9cee0b050..049ef6dcb 100755 --- a/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py +++ b/examples/python/gRPC/meta/gRPC_pb_getOverallBound.py @@ -43,7 +43,7 @@ uuiddt_category = uuid_datatype_with_category.UuidDatatypeWithCategory() uuiddt_category.category = "3" uuiddt_category.uuid_with_datatype.projectuuid = projectuuid -uuiddt_category.uuid_with_datatype.datatype = datatype.image +uuiddt_category.uuid_with_datatype.datatype = datatype.all response = stub.GetAllLabels(uuiddt_category) print(response) diff --git a/seerep_msgs/core/datatype.h b/seerep_msgs/core/datatype.h index a3b7c5091..5f00a2a74 100644 --- a/seerep_msgs/core/datatype.h +++ b/seerep_msgs/core/datatype.h @@ -9,7 +9,8 @@ enum class Datatype Unknown, // default value Image, PointCloud, - Point + Point, + All }; } /* namespace seerep_core_msgs */ diff --git a/seerep_msgs/fbs/datatype.fbs b/seerep_msgs/fbs/datatype.fbs index efbc418ba..d20e3ffc8 100644 --- a/seerep_msgs/fbs/datatype.fbs +++ b/seerep_msgs/fbs/datatype.fbs @@ -5,5 +5,6 @@ enum Datatype:byte { Unknown=0, Image, PointCloud, - Point + Point, + All } diff --git a/seerep_msgs/protos/datatype.proto b/seerep_msgs/protos/datatype.proto index ff199dfe8..0c404722e 100644 --- a/seerep_msgs/protos/datatype.proto +++ b/seerep_msgs/protos/datatype.proto @@ -8,4 +8,5 @@ enum datatype image = 1; pointcloud = 2; point = 3; + all = 4; } diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index fa6c33d64..f817e687b 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -41,6 +41,8 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service private: std::shared_ptr seerepCore; boost::log::sources::severity_logger m_logger; + + std::vector convertFbDatatypeVector(const seerep::fb::Datatype dt); }; } /* namespace seerep_server */ diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h index f1e6b591f..46fe20a2f 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h @@ -34,6 +34,8 @@ class PbMetaOperations final : public seerep::pb::MetaOperations::Service private: std::shared_ptr seerepCore; boost::log::sources::severity_logger m_logger; + + std::vector convertPbDatatypeVector(const seerep::datatype dt); }; } /* namespace seerep_server */ diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 84be19a7d..8a211157c 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -183,22 +183,7 @@ FbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* context, std::vector dt_vector; seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatype()); - if (casted_datatype == seerep::fb::Datatype_Image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (casted_datatype == seerep::fb::Datatype_PointCloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (casted_datatype == seerep::fb::Datatype_Point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertFbDatatypeVector(casted_datatype); try { @@ -280,23 +265,11 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, // levels. std::vector dt_vector; + // flatbuffers::IsFieldPresent(requestRoot, seerep::fb::UuidDatatypePair::datatype); + // requestRoot->CheckField(seerep::fb::UuidDatatypePair::datatype); + seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatype()); - if (casted_datatype == seerep::fb::Datatype_Image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (casted_datatype == seerep::fb::Datatype_PointCloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (casted_datatype == seerep::fb::Datatype_Point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertFbDatatypeVector(casted_datatype); try { @@ -380,22 +353,7 @@ grpc::Status FbMetaOperations::GetAllCategories(grpc::ServerContext* context, std::vector dt_vector; seerep::fb::Datatype casted_datatype = static_cast(requestRoot->datatype()); - if (casted_datatype == seerep::fb::Datatype_Image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (casted_datatype == seerep::fb::Datatype_PointCloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (casted_datatype == seerep::fb::Datatype_Point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertFbDatatypeVector(casted_datatype); try { @@ -466,22 +424,7 @@ FbMetaOperations::GetAllLabels(grpc::ServerContext* context, std::vector dt_vector; seerep::fb::Datatype casted_datatype = static_cast(requestRoot->UuidAndDatatype()->datatype()); - if (casted_datatype == seerep::fb::Datatype_Image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (casted_datatype == seerep::fb::Datatype_PointCloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (casted_datatype == seerep::fb::Datatype_Point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertFbDatatypeVector(casted_datatype); try { @@ -535,4 +478,34 @@ FbMetaOperations::GetAllLabels(grpc::ServerContext* context, return grpc::Status::OK; } +std::vector FbMetaOperations::convertFbDatatypeVector(const seerep::fb::Datatype dt) +{ + std::vector dt_vector; + + if (dt == seerep::fb::Datatype_Image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (dt == seerep::fb::Datatype_PointCloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (dt == seerep::fb::Datatype_Point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else if (dt == seerep::fb::Datatype_All) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + return dt_vector; +} + } /* namespace seerep_server */ diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 38ddee0b4..d2231c411 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -102,23 +102,7 @@ grpc::Status PbMetaOperations::GetOverallTimeInterval(grpc::ServerContext* conte BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall time interval"; std::vector dt_vector; - - if (request->datatype() == seerep::datatype::image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (request->datatype() == seerep::datatype::pointcloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (request->datatype() == seerep::datatype::point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertPbDatatypeVector(request->datatype()); try { @@ -169,23 +153,7 @@ grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* contex BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; std::vector dt_vector; - - if (request->datatype() == seerep::datatype::image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (request->datatype() == seerep::datatype::pointcloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (request->datatype() == seerep::datatype::point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertPbDatatypeVector(request->datatype()); try { @@ -241,23 +209,7 @@ grpc::Status PbMetaOperations::GetAllCategories(grpc::ServerContext* context, BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching all categories"; std::vector dt_vector; - - if (request->datatype() == seerep::datatype::image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (request->datatype() == seerep::datatype::pointcloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (request->datatype() == seerep::datatype::point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertPbDatatypeVector(request->datatype()); try { @@ -298,23 +250,7 @@ grpc::Status PbMetaOperations::GetAllLabels(grpc::ServerContext* context, BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; std::vector dt_vector; - - if (request->uuid_with_datatype().datatype() == seerep::datatype::image) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Image); - } - else if (request->uuid_with_datatype().datatype() == seerep::datatype::pointcloud) - { - dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); - } - else if (request->uuid_with_datatype().datatype() == seerep::datatype::point) - { - dt_vector.push_back(seerep_core_msgs::Datatype::Point); - } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } + dt_vector = convertPbDatatypeVector(request->uuid_with_datatype().datatype()); try { @@ -349,4 +285,34 @@ grpc::Status PbMetaOperations::GetAllLabels(grpc::ServerContext* context, return grpc::Status::OK; } +std::vector PbMetaOperations::convertPbDatatypeVector(const seerep::datatype datatype) +{ + std::vector dt_vector; + + if (datatype == seerep::datatype::image) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + } + else if (datatype == seerep::datatype::pointcloud) + { + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + } + else if (datatype == seerep::datatype::point) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else if (datatype == seerep::datatype::all) + { + dt_vector.push_back(seerep_core_msgs::Datatype::Image); + dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); + dt_vector.push_back(seerep_core_msgs::Datatype::Point); + } + else + { + dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); + } + + return dt_vector; +} + } /* namespace seerep_server */ From c139874cda28c8fe0fbec23034479142c1dc04d7 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Wed, 3 May 2023 17:58:05 +0200 Subject: [PATCH 21/21] removed datatype unknown from pb/fb level --- seerep_msgs/fbs/datatype.fbs | 1 - seerep_msgs/protos/datatype.proto | 9 ++++----- seerep_srv/seerep_server/src/fb_meta_operations.cpp | 4 ---- seerep_srv/seerep_server/src/pb_meta_operations.cpp | 4 ---- 4 files changed, 4 insertions(+), 14 deletions(-) diff --git a/seerep_msgs/fbs/datatype.fbs b/seerep_msgs/fbs/datatype.fbs index d20e3ffc8..1e9c3e89e 100644 --- a/seerep_msgs/fbs/datatype.fbs +++ b/seerep_msgs/fbs/datatype.fbs @@ -2,7 +2,6 @@ namespace seerep.fb; enum Datatype:byte { - Unknown=0, Image, PointCloud, Point, diff --git a/seerep_msgs/protos/datatype.proto b/seerep_msgs/protos/datatype.proto index 0c404722e..ffa261b9d 100644 --- a/seerep_msgs/protos/datatype.proto +++ b/seerep_msgs/protos/datatype.proto @@ -4,9 +4,8 @@ package seerep; enum datatype { - unknown = 0; - image = 1; - pointcloud = 2; - point = 3; - all = 4; + image = 0; + pointcloud = 1; + point = 2; + all = 3; } diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 8a211157c..30f328c70 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -500,10 +500,6 @@ std::vector FbMetaOperations::convertFbDatatypeVecto dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); dt_vector.push_back(seerep_core_msgs::Datatype::Point); } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } return dt_vector; } diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index d2231c411..f0b94ebd5 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -307,10 +307,6 @@ std::vector PbMetaOperations::convertPbDatatypeVecto dt_vector.push_back(seerep_core_msgs::Datatype::PointCloud); dt_vector.push_back(seerep_core_msgs::Datatype::Point); } - else - { - dt_vector.push_back(seerep_core_msgs::Datatype::Unknown); - } return dt_vector; }