From 0eb1ac6f75856e30517265b6198439ceb0e71543 Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Thu, 21 Nov 2024 15:36:23 +0100 Subject: [PATCH] Revert "WIP: query for multiple time intervals at once" This reverts commit ad6ead9c8eae60061897f34055e9437d2b3bb56f. --- .../python/gRPC/images/gRPC_fb_queryImages.py | 11 ++---- seerep_msgs/core/query.h | 3 +- seerep_msgs/fbs/query.fbs | 2 +- seerep_msgs/protos/query.proto | 2 +- seerep_srv/seerep_core/src/core_dataset.cpp | 29 +++++++-------- .../seerep_core_fb/core_fb_conversion.h | 6 ++-- .../seerep_core_fb/src/core_fb_conversion.cpp | 30 ++++++++-------- .../seerep_core_pb/src/core_pb_conversion.cpp | 26 ++++++-------- .../seerep_server/src/fb_image_service.cpp | 18 +++++----- .../src/fb_point_cloud_service.cpp | 19 +++++----- .../seerep_server/src/fb_point_service.cpp | 36 +++++++++++++------ .../seerep_server/src/pb_image_service.cpp | 13 +++---- .../seerep_server/src/pb_meta_operations.cpp | 2 +- .../src/pb_point_cloud_service.cpp | 13 +++---- 14 files changed, 100 insertions(+), 110 deletions(-) diff --git a/examples/python/gRPC/images/gRPC_fb_queryImages.py b/examples/python/gRPC/images/gRPC_fb_queryImages.py index ace7bac24..79f0aab4f 100755 --- a/examples/python/gRPC/images/gRPC_fb_queryImages.py +++ b/examples/python/gRPC/images/gRPC_fb_queryImages.py @@ -92,13 +92,9 @@ def query_images( polygon_2d = createPolygon2D(fbb, 700, -100, vertices) - time_intervals = [] - time_min = createTimeStamp(fbb, 1661336500, 0) - time_max = createTimeStamp(fbb, 1661336550, 0) - time_intervals.append(createTimeInterval(fbb, time_min, time_max)) - time_min = createTimeStamp(fbb, 1661336560, 0) - time_max = createTimeStamp(fbb, 1661336600, 0) - time_intervals.append(createTimeInterval(fbb, time_min, time_max)) + time_min = createTimeStamp(fbb, 1610549273, 0) + time_max = createTimeStamp(fbb, 1938549273, 0) + time_interval = createTimeInterval(fbb, time_min, time_max) project_uuids = [project_uuid] @@ -120,7 +116,6 @@ def query_images( fbb, grpc_channel, project_uuid, - timeIntervals=time_intervals, # polygon2d=polygon_2d, polygon2dSensorPos=polygon_2d, labels=labelsCategory, diff --git a/seerep_msgs/core/query.h b/seerep_msgs/core/query.h index 3fd587555..b6713db54 100644 --- a/seerep_msgs/core/query.h +++ b/seerep_msgs/core/query.h @@ -25,8 +25,7 @@ struct Query // polygon defined above bool inMapFrame; // if false the query polygon is in geodetic coordinates, // otherwise in map frame - std::optional> - timeintervals; ///< only do temporal query if set + std::optional timeinterval; ///< only do temporal query if set std::optional>> label; ///< only do semantic query if set std::optional diff --git a/seerep_msgs/fbs/query.fbs b/seerep_msgs/fbs/query.fbs index c4cafe75d..9b12d9d11 100644 --- a/seerep_msgs/fbs/query.fbs +++ b/seerep_msgs/fbs/query.fbs @@ -10,7 +10,7 @@ table Query { polygonSensorPosition:Polygon2D; fullyEncapsulated:bool; inMapFrame:bool; - timeintervals:[TimeInterval]; + timeinterval:TimeInterval; label:[LabelCategory]; sparqlQuery:SparqlQuery; ontologyURI:string; diff --git a/seerep_msgs/protos/query.proto b/seerep_msgs/protos/query.proto index d67466830..ca0a28ee1 100644 --- a/seerep_msgs/protos/query.proto +++ b/seerep_msgs/protos/query.proto @@ -11,7 +11,7 @@ message Query Polygon2D polygon = 1; bool fullyEncapsulated = 2; bool inMapFrame = 3; - repeated TimeInterval timeintervals = 4; + TimeInterval timeinterval = 4; repeated LabelCategory labelCategory = 5; bool mustHaveAllLabels = 6; repeated string projectuuid = 7; diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index cada2ac3f..02f933688 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -143,7 +143,7 @@ CoreDataset::getData(const seerep_core_msgs::Query& query) } } - if (!query.polygon && !query.polygonSensorPos && !query.timeintervals && + if (!query.polygon && !query.polygonSensorPos && !query.timeinterval && !query.label && !query.instances && !query.dataUuids) { return getAllDatasetUuids(datatypeSpecifics, query.sortByTime); @@ -335,24 +335,21 @@ std::optional> CoreDataset::queryTemporal(std::shared_ptr datatypeSpecifics, const seerep_core_msgs::Query& query) { - if (query.timeintervals) + if (query.timeinterval) { std::optional> timetree_result = std::vector(); - for (auto timeinterval : query.timeintervals.value()) - { - seerep_core_msgs::AabbTime aabbtime( - seerep_core_msgs::TimePoint(((int64_t)timeinterval.timeMin.seconds) - << 32 | - ((uint64_t)timeinterval.timeMin.nanos)), - seerep_core_msgs::TimePoint(((int64_t)timeinterval.timeMax.seconds) - << 32 | - ((uint64_t)timeinterval.timeMax.nanos))); - - datatypeSpecifics->timetree.query( - boost::geometry::index::intersects(aabbtime), - std::back_inserter(timetree_result.value())); - } + seerep_core_msgs::AabbTime aabbtime( + seerep_core_msgs::TimePoint( + ((int64_t)query.timeinterval.value().timeMin.seconds) << 32 | + ((uint64_t)query.timeinterval.value().timeMin.nanos)), + seerep_core_msgs::TimePoint( + ((int64_t)query.timeinterval.value().timeMax.seconds) << 32 | + ((uint64_t)query.timeinterval.value().timeMax.nanos))); + + datatypeSpecifics->timetree.query( + boost::geometry::index::intersects(aabbtime), + std::back_inserter(timetree_result.value())); return timetree_result; } else diff --git a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h index aee853e67..fa1da5ae3 100644 --- a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h +++ b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h @@ -262,9 +262,9 @@ class CoreFbConversion * @param query the flatbuffer query message * @param queryCoreTime the time in the query message in seerep core format */ - static void fromFbQueryTime( - const seerep::fb::Query* query, - std::optional>& queryCoreTime); + static void + fromFbQueryTime(const seerep::fb::Query* query, + std::optional& queryCoreTime); /** * @brief extracts the WithoutData Flag of the flatbuffer query message * @param query the flatbuffer query message diff --git a/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp b/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp index bd7baec91..f46f88bbd 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp @@ -37,7 +37,7 @@ CoreFbConversion::fromFb(const seerep::fb::Query* query, seerep_core_msgs::Query queryCore; queryCore.header.datatype = datatype; - fromFbQueryTime(query, queryCore.timeintervals); + fromFbQueryTime(query, queryCore.timeinterval); fromFbQueryLabel(query, queryCore.label); queryCore.sparqlQuery = fromFbSparqlQuery(query); queryCore.ontologyURI = fromFbOntologyURI(query); @@ -520,21 +520,19 @@ void CoreFbConversion::fromFbQueryLabel( void CoreFbConversion::fromFbQueryTime( const seerep::fb::Query* query, - std::optional>& queryCoreTimes) -{ - if (flatbuffers::IsFieldPresent(query, seerep::fb::Query::VT_TIMEINTERVALS)) - { - queryCoreTimes = std::vector(); - for (auto&& timeinterval : *query->timeintervals()) - { - seerep_core_msgs::Timeinterval queryCoreTime = - seerep_core_msgs::Timeinterval(); - queryCoreTime.timeMin.seconds = timeinterval->time_min()->seconds(); - queryCoreTime.timeMax.seconds = timeinterval->time_max()->seconds(); - queryCoreTime.timeMin.nanos = timeinterval->time_min()->nanos(); - queryCoreTime.timeMax.nanos = timeinterval->time_max()->nanos(); - queryCoreTimes.value().push_back(queryCoreTime); - } + std::optional& queryCoreTime) +{ + if (flatbuffers::IsFieldPresent(query, seerep::fb::Query::VT_TIMEINTERVAL)) + { + queryCoreTime = seerep_core_msgs::Timeinterval(); + queryCoreTime.value().timeMin.seconds = + query->timeinterval()->time_min()->seconds(); + queryCoreTime.value().timeMax.seconds = + query->timeinterval()->time_max()->seconds(); + queryCoreTime.value().timeMin.nanos = + query->timeinterval()->time_min()->nanos(); + queryCoreTime.value().timeMax.nanos = + query->timeinterval()->time_max()->nanos(); } } diff --git a/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp b/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp index 983aacea5..f9df44081 100644 --- a/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp +++ b/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp @@ -324,23 +324,17 @@ void CorePbConversion::fromPbLabel(const seerep::pb::Query& query, void CorePbConversion::fromPbTime(const seerep::pb::Query& query, seerep_core_msgs::Query& queryCore) { - if (query.timeintervals_size() > 0) + if (query.timeinterval().has_time_min() && query.timeinterval().has_time_max()) { - queryCore.timeintervals = std::vector(); - for (auto&& timeinterval : query.timeintervals()) - { - if (timeinterval.has_time_min() && timeinterval.has_time_max()) - { - seerep_core_msgs::Timeinterval queryCoreTime = - seerep_core_msgs::Timeinterval(); - queryCoreTime.timeMin.seconds = timeinterval.time_min().seconds(); - queryCoreTime.timeMax.seconds = timeinterval.time_max().seconds(); - queryCoreTime.timeMin.nanos = timeinterval.time_min().nanos(); - queryCoreTime.timeMax.nanos = timeinterval.time_max().nanos(); - - queryCore.timeintervals.value().push_back(queryCoreTime); - } - } + queryCore.timeinterval = seerep_core_msgs::Timeinterval(); + queryCore.timeinterval.value().timeMin.seconds = + query.timeinterval().time_min().seconds(); + queryCore.timeinterval.value().timeMax.seconds = + query.timeinterval().time_max().seconds(); + queryCore.timeinterval.value().timeMin.nanos = + query.timeinterval().time_min().nanos(); + queryCore.timeinterval.value().timeMax.nanos = + query.timeinterval().time_max().nanos(); } } diff --git a/seerep_srv/seerep_server/src/fb_image_service.cpp b/seerep_srv/seerep_server/src/fb_image_service.cpp index cc90ad210..9b4d1d6e4 100644 --- a/seerep_srv/seerep_server/src/fb_image_service.cpp +++ b/seerep_srv/seerep_server/src/fb_image_service.cpp @@ -19,7 +19,7 @@ grpc::Status FbImageService::GetImage( debuginfo << "sending images with this query parameters:"; if (requestRoot->polygon() != NULL) { - for (auto&& point : *requestRoot->polygon()->vertices()) + for (auto point : *requestRoot->polygon()->vertices()) { debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() << ") /"; @@ -30,7 +30,7 @@ grpc::Status FbImageService::GetImage( } if (requestRoot->polygonSensorPosition() != NULL) { - for (auto&& point : *(requestRoot->polygonSensorPosition()->vertices())) + for (auto point : *(requestRoot->polygonSensorPosition()->vertices())) { debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() << ") /"; @@ -40,21 +40,19 @@ grpc::Status FbImageService::GetImage( debuginfo << "bounding box height " << requestRoot->polygonSensorPosition()->height() << " /"; } - if (requestRoot->timeintervals() != NULL) + if (requestRoot->timeinterval() != NULL) { - for (auto&& timeinterval : *requestRoot->timeintervals()) - { - debuginfo << "\n time interval (" << timeinterval->time_min()->seconds() - << "/" << timeinterval->time_max()->seconds() << ")"; - } + debuginfo << "\n time interval (" + << requestRoot->timeinterval()->time_min()->seconds() << "/" + << requestRoot->timeinterval()->time_max()->seconds() << ")"; } if (requestRoot->label() != NULL) { debuginfo << "\n labels general"; - for (auto&& labelCategory : *requestRoot->label()) + for (auto labelCategory : *requestRoot->label()) { debuginfo << "category: " << labelCategory->category()->c_str() << "; "; - for (auto&& label : *labelCategory->labels()) + for (auto label : *labelCategory->labels()) { debuginfo << "'" << label->label()->str() << "' "; } diff --git a/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp b/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp index 9c58441b1..ccb14e7f0 100644 --- a/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp +++ b/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp @@ -24,7 +24,7 @@ grpc::Status FbPointCloudService::GetPointCloud2( debuginfo << "sending point clouds with this query parameters: "; if (requestRoot->polygon() != NULL) { - for (auto&& point : *requestRoot->polygon()->vertices()) + for (auto point : *requestRoot->polygon()->vertices()) { debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() << ") /"; @@ -35,7 +35,7 @@ grpc::Status FbPointCloudService::GetPointCloud2( } if (requestRoot->polygonSensorPosition() != NULL) { - for (auto&& point : *(requestRoot->polygonSensorPosition()->vertices())) + for (auto point : *(requestRoot->polygonSensorPosition()->vertices())) { debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() << ") /"; @@ -45,21 +45,20 @@ grpc::Status FbPointCloudService::GetPointCloud2( debuginfo << "bounding box height " << requestRoot->polygonSensorPosition()->height() << " /"; } - if (requestRoot->timeintervals() != NULL) + if (requestRoot->timeinterval() != NULL) { - for (auto&& timeinterval : *requestRoot->timeintervals()) - { - debuginfo << "\n time interval (" << timeinterval->time_min()->seconds() - << "/" << timeinterval->time_max()->seconds() << ")"; - } + debuginfo << "\n time interval (seconds since epoch: " + << requestRoot->timeinterval()->time_min()->seconds() + << ", nanoseconds since epoch:" + << requestRoot->timeinterval()->time_max()->seconds() << ")"; } if (requestRoot->label() != NULL) { debuginfo << "\n labels general"; - for (auto&& labelCategory : *requestRoot->label()) + for (auto labelCategory : *requestRoot->label()) { debuginfo << "category: " << labelCategory->category()->c_str() << "; "; - for (auto&& label : *labelCategory->labels()) + for (auto label : *labelCategory->labels()) { debuginfo << "'" << label->label()->str() << "' "; } diff --git a/seerep_srv/seerep_server/src/fb_point_service.cpp b/seerep_srv/seerep_server/src/fb_point_service.cpp index d8af72ee5..68ac14364 100644 --- a/seerep_srv/seerep_server/src/fb_point_service.cpp +++ b/seerep_srv/seerep_server/src/fb_point_service.cpp @@ -20,7 +20,7 @@ grpc::Status FbPointService::GetPoint( debuginfo << "sending images with this query parameters:"; if (requestRoot->polygon() != NULL) { - for (auto&& point : *requestRoot->polygon()->vertices()) + for (auto point : *requestRoot->polygon()->vertices()) { debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() << ") \n"; @@ -31,7 +31,7 @@ grpc::Status FbPointService::GetPoint( } if (requestRoot->polygonSensorPosition() != NULL) { - for (auto&& point : *(requestRoot->polygonSensorPosition()->vertices())) + for (auto point : *(requestRoot->polygonSensorPosition()->vertices())) { debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() << ") /"; @@ -41,21 +41,19 @@ grpc::Status FbPointService::GetPoint( debuginfo << "bounding box height " << requestRoot->polygonSensorPosition()->height() << " /"; } - if (requestRoot->timeintervals() != NULL) + if (requestRoot->timeinterval() != NULL) { - for (auto&& timeinterval : *requestRoot->timeintervals()) - { - debuginfo << "\n time interval (" << timeinterval->time_min()->seconds() - << "/" << timeinterval->time_max()->seconds() << ")"; - } + debuginfo << "\n time interval (" + << requestRoot->timeinterval()->time_min()->seconds() << "/" + << requestRoot->timeinterval()->time_max()->seconds() << ")"; } if (requestRoot->label() != NULL) { debuginfo << "\n labels general"; - for (auto&& labelCategory : *requestRoot->label()) + for (auto labelCategory : *requestRoot->label()) { debuginfo << "category: " << labelCategory->category()->c_str() << "; "; - for (auto&& label : *labelCategory->labels()) + for (auto label : *labelCategory->labels()) { debuginfo << "'" << label->label()->str() << "' "; } @@ -65,6 +63,24 @@ grpc::Status FbPointService::GetPoint( BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << debuginfo.rdbuf(); + if (requestRoot->polygon() != NULL) + { + for (auto point : *(requestRoot->polygon()->vertices())) + { + debuginfo << "bounding box vertex (" << point->x() << ", " << point->y() + << ") /"; + } + debuginfo << "bounding box z " << requestRoot->polygon()->z() << " /"; + debuginfo << "bounding box height " << requestRoot->polygon()->height() + << " /"; + } + if (requestRoot->timeinterval() != NULL) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "in time interval (" + << requestRoot->timeinterval()->time_min()->seconds() << "/" + << requestRoot->timeinterval()->time_max()->seconds() << ")"; + } try { pointFb->getData(requestRoot, writer); diff --git a/seerep_srv/seerep_server/src/pb_image_service.cpp b/seerep_srv/seerep_server/src/pb_image_service.cpp index d6f5fd20f..6e5f1a1a0 100644 --- a/seerep_srv/seerep_server/src/pb_image_service.cpp +++ b/seerep_srv/seerep_server/src/pb_image_service.cpp @@ -14,15 +14,12 @@ PbImageService::GetImage(grpc::ServerContext* context, { (void)context; // ignore that variable without causing warnings // print time bound to debug log - for (auto&& timeinterval : request->timeintervals()) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "sending point cloud in time interval (" - << timeinterval.time_min().seconds() << "/" - << timeinterval.time_max().seconds() << ")"; - } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "sending point cloud in time interval (" + << request->timeinterval().time_min().seconds() << "/" + << request->timeinterval().time_max().seconds() << ")"; // print polygon vertices to debug log - for (auto&& point : request->polygon().vertices()) + for (auto point : request->polygon().vertices()) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "bounding box vertex (" << point.x() << ", " << point.y() << ") /"; diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 43af0c7f6..5eb309567 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -84,7 +84,7 @@ PbMetaOperations::GetProjects(grpc::ServerContext* context, { auto projectInfos = seerepCore->getProjects(); - for (auto&& projectInfo : projectInfos) + for (auto projectInfo : projectInfos) { auto responseProjectInfo = response->add_projects(); responseProjectInfo->set_name(projectInfo.name); diff --git a/seerep_srv/seerep_server/src/pb_point_cloud_service.cpp b/seerep_srv/seerep_server/src/pb_point_cloud_service.cpp index 63fbd6ede..c44f34428 100644 --- a/seerep_srv/seerep_server/src/pb_point_cloud_service.cpp +++ b/seerep_srv/seerep_server/src/pb_point_cloud_service.cpp @@ -14,15 +14,12 @@ grpc::Status PbPointCloudService::GetPointCloud2( { (void)context; // ignore that variable without causing warnings // print time bound to debug log - for (auto&& timeinterval : request->timeintervals()) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "sending point cloud in time interval (" - << timeinterval.time_min().seconds() << "/" - << timeinterval.time_max().seconds() << ")"; - } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "sending point cloud in time interval (" + << request->timeinterval().time_min().seconds() << "/" + << request->timeinterval().time_max().seconds() << ")"; // print polygon vertices to debug log - for (auto&& point : request->polygon().vertices()) + for (auto point : request->polygon().vertices()) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "bounding box vertex (" << point.x() << ", " << point.y() << ") /";