Merge pull request #348 from agri-gaia/feat/querySortedByTime

Feat/query sorted by time

.gitignore

@@ -59,3 +59,5 @@ wheels/
 .installed.cfg
 *.egg
 MANIFEST
+
+**/other_packages/**

examples/python/gRPC/util/fb_helper.py

@@ -486,6 +486,7 @@ def createQuery(
     polygon2d=None,
     fullyEncapsulated=False,
     inMapFrame=True,
+    sortByTime=False,
 ):
     '''Create a query, all parameters are optional'''
 
@@ -527,6 +528,7 @@ def createQuery(
     Query.AddWithoutdata(builder, withoutData)
     Query.AddFullyEncapsulated(builder, fullyEncapsulated)
     Query.AddInMapFrame(builder, inMapFrame)
+    Query.AddSortByTime(builder, sortByTime)
 
     return Query.End(builder)
 

seerep_msgs/core/query.h

@@ -28,6 +28,7 @@ struct Query
   std::optional<std::vector<boost::uuids::uuid>> dataUuids;  ///< only filter by data uuid if set
   bool withoutData;                                          ///< do not return the data itself if set
   uint maxNumData;                                           ///< max number of datasets that should be returned
+  bool sortByTime;  ///< sort the resulting uuids by the timestamp of the corresponding data
 };
 
 } /* namespace seerep_core_msgs */

seerep_msgs/fbs/query.fbs

@@ -20,4 +20,5 @@ table Query {
   datauuid:[string];
   withoutdata:bool;
   maxNumData:uint;
+  sortByTime:bool;
 }

seerep_msgs/protos/query.proto

@@ -19,4 +19,5 @@ message Query
   repeated string datauuid = 9;
   bool withoutdata = 10;
   uint32 maxNumData = 11;
+  bool sortByTime = 12;
 }

seerep_srv/seerep_core/include/seerep_core/core_dataset.h

@@ -290,22 +290,33 @@ class CoreDataset
                         std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>>& timetree_result,
                         std::optional<std::set<boost::uuids::uuid>>& semanticResult,
                         std::optional<std::vector<boost::uuids::uuid>>& instanceResult,
-                        const std::optional<std::vector<boost::uuids::uuid>>& dataUuids);
+                        const std::optional<std::vector<boost::uuids::uuid>>& dataUuids, bool sortByTime = false);
+  /**
+   * @brief sorts the uuids of the result set based on their timestamp
+   *
+   * @param timetree_result the result from the timetree query. Contains uuids and the corresponding timestamp
+   * @param intersectedResult the intersected uuids of all parts of the query
+   * @return std::vector<boost::uuids::uuid> uuids of the result set sorted by timestamp
+   */
+  std::vector<boost::uuids::uuid>
+  sortResultByTime(std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>>& timetree_result,
+                   std::optional<std::set<boost::uuids::uuid>> intersectionResult = std::nullopt);
 
   /**
    * @brief intersects a vector of sets pairwise recursively until one intersection set remains
    * @param vectorOfSets the vector of sets to be intersected
-   * @return vector of UUIDs of the intersection result
+   * @return set of UUIDs of the intersection result
    */
-  std::vector<boost::uuids::uuid> intersectVectorOfSets(std::vector<std::set<boost::uuids::uuid>>& vectorOfSets);
+  std::set<boost::uuids::uuid> intersectVectorOfSets(std::vector<std::set<boost::uuids::uuid>>& vectorOfSets);
 
   /**
    * @brief return the UUIDs of all stored datasets. Uses the timeTree, because all datasets are in there
    * @param datatypeSpecifics the datatype specifics of the targeted data type
+   * @param sortByTime flag if the result set should be sorted by the timestamp of the data
    * @return vector of UUIDs of all data sets
    */
   std::vector<boost::uuids::uuid>
-  getAllDatasetUuids(std::shared_ptr<seerep_core::CoreDataset::DatatypeSpecifics> datatypeSpecifics);
+  getAllDatasetUuids(std::shared_ptr<seerep_core::CoreDataset::DatatypeSpecifics> datatypeSpecifics, bool sortByTime);
 
   /** @brief the frame id of the spatial index*/
   std::string m_frameId;

seerep_srv/seerep_core/src/core.cpp

@@ -375,7 +375,7 @@ void Core::checkForOntologyConcepts(seerep_core_msgs::Query& query)
     {
       // only iterate over the initial size of the vector
       auto startSizeLabels = category.second.size();
-      for (int i = 0; i < startSizeLabels; i++)
+      for (std::size_t i = 0; i < startSizeLabels; i++)
       {
         std::string& label = category.second.at(i);
         // check if label is NOT URL (ontology concept)

seerep_srv/seerep_core/src/core_dataset.cpp

@@ -80,10 +80,10 @@ std::vector<boost::uuids::uuid> CoreDataset::getData(const seerep_core_msgs::Que
 
   if (!query.polygon && !query.timeinterval && !query.label && !query.instances && !query.dataUuids)
   {
-    return getAllDatasetUuids(datatypeSpecifics);
+    return getAllDatasetUuids(datatypeSpecifics, query.sortByTime);
   }
 
-  return intersectQueryResults(resultRt, resultTime, resultSemantic, instanceResult, query.dataUuids);
+  return intersectQueryResults(resultRt, resultTime, resultSemantic, instanceResult, query.dataUuids, query.sortByTime);
 }
 
 std::vector<boost::uuids::uuid> CoreDataset::getInstances(const seerep_core_msgs::Query& query)
@@ -356,7 +356,7 @@ CoreDataset::intersectQueryResults(std::optional<std::vector<seerep_core_msgs::A
                                    std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>>& timetree_result,
                                    std::optional<std::set<boost::uuids::uuid>>& semanticResult,
                                    std::optional<std::vector<boost::uuids::uuid>>& instanceResult,
-                                   const std::optional<std::vector<boost::uuids::uuid>>& dataUuids)
+                                   const std::optional<std::vector<boost::uuids::uuid>>& dataUuids, bool sortByTime)
 {
   std::vector<std::set<boost::uuids::uuid>> idsPerSingleModality;
 
@@ -379,7 +379,7 @@ CoreDataset::intersectQueryResults(std::optional<std::vector<seerep_core_msgs::A
               end = std::make_move_iterator(timetree_result.value().end());
          it != end; ++it)
     {
-      idsTemporal.insert(std::move(it->second));
+      idsTemporal.insert(it->second);
     }
     idsPerSingleModality.push_back(std::move(idsTemporal));
   }
@@ -401,11 +401,53 @@ CoreDataset::intersectQueryResults(std::optional<std::vector<seerep_core_msgs::A
         std::move(std::set<boost::uuids::uuid>(dataUuids.value().begin(), dataUuids.value().end())));
   }
 
-  return intersectVectorOfSets(idsPerSingleModality);
+  auto intersectionResult = intersectVectorOfSets(idsPerSingleModality);
+
+  // sort by timestamp
+  if (sortByTime)
+  {
+    if (timetree_result.has_value())
+    {
+      return sortResultByTime(timetree_result, intersectionResult);
+    }
+    else
+    {
+      BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning)
+          << "Results will not be sorted by time (even though it was requested), because there is no temporal part in "
+             "the query";
+    }
+  }
+
+  return std::vector(intersectionResult.begin(), intersectionResult.end());
 }
 
 std::vector<boost::uuids::uuid>
-CoreDataset::intersectVectorOfSets(std::vector<std::set<boost::uuids::uuid>>& vectorOfSets)
+CoreDataset::sortResultByTime(std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>>& timetree_result,
+                              std::optional<std::set<boost::uuids::uuid>> intersectionResult)
+{
+  std::vector<std::pair<int64_t, boost::uuids::uuid>> sortingVector;
+  for (auto it = std::make_move_iterator(timetree_result.value().begin()),
+            end = std::make_move_iterator(timetree_result.value().end());
+       it != end; ++it)
+  {
+    if (!intersectionResult.has_value() ||
+        intersectionResult.value().find(it->second) != intersectionResult.value().end())
+    {
+      sortingVector.push_back(std::make_pair(boost::geometry::get<0>(it->first.min_corner()), it->second));
+    }
+  }
+
+  std::sort(sortingVector.begin(), sortingVector.end());
+  std::vector<boost::uuids::uuid> sortedResults;
+  sortedResults.reserve(sortingVector.size());
+  for (auto x : sortingVector)
+  {
+    sortedResults.push_back(x.second);
+  }
+  return sortedResults;
+}
+
+std::set<boost::uuids::uuid> CoreDataset::intersectVectorOfSets(std::vector<std::set<boost::uuids::uuid>>& vectorOfSets)
 {
   if (vectorOfSets.size() > 1)
   {
@@ -430,32 +472,42 @@ CoreDataset::intersectVectorOfSets(std::vector<std::set<boost::uuids::uuid>>& ve
   else if (vectorOfSets.size() == 1)
   {
     // return the result as soon as all sets are intersected
-    return std::vector(vectorOfSets.at(0).begin(), vectorOfSets.at(0).end());
+    return vectorOfSets.at(0);
   }
 
   // return empty vector if input vector is empty
-  return std::vector<boost::uuids::uuid>();
+  return std::set<boost::uuids::uuid>();
 }
 
 std::vector<boost::uuids::uuid>
-CoreDataset::getAllDatasetUuids(std::shared_ptr<seerep_core::CoreDataset::DatatypeSpecifics> datatypeSpecifics)
+CoreDataset::getAllDatasetUuids(std::shared_ptr<seerep_core::CoreDataset::DatatypeSpecifics> datatypeSpecifics,
+                                bool sortByTime)
 {
-  std::vector<seerep_core_msgs::AabbTimeIdPair> allIdsFromTimeTree = std::vector<seerep_core_msgs::AabbTimeIdPair>();
+  std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>> allIdsFromTimeTree =
+      std::vector<seerep_core_msgs::AabbTimeIdPair>();
   seerep_core_msgs::AabbTime aabbtime(seerep_core_msgs::TimePoint(((int64_t)std::numeric_limits<uint32_t>::min() << 32 |
                                                                    ((uint64_t)std::numeric_limits<uint32_t>::min()))),
                                       seerep_core_msgs::TimePoint(((int64_t)std::numeric_limits<int32_t>::max()) << 32 |
                                                                   ((uint64_t)std::numeric_limits<uint32_t>::max())));
 
   datatypeSpecifics->timetree.query(boost::geometry::index::intersects(aabbtime),
-                                    std::back_inserter(allIdsFromTimeTree));
-  std::vector<boost::uuids::uuid> allIds;
-  for (auto it = std::make_move_iterator(allIdsFromTimeTree.begin()),
-            end = std::make_move_iterator(allIdsFromTimeTree.end());
-       it != end; ++it)
+                                    std::back_inserter(allIdsFromTimeTree.value()));
+
+  if (sortByTime)
+  {
+    return sortResultByTime(allIdsFromTimeTree);
+  }
+  else
   {
-    allIds.push_back(std::move(it->second));
+    std::vector<boost::uuids::uuid> allIds;
+    for (auto it = std::make_move_iterator(allIdsFromTimeTree.value().begin()),
+              end = std::make_move_iterator(allIdsFromTimeTree.value().end());
+         it != end; ++it)
+    {
+      allIds.push_back(std::move(it->second));
+    }
+    return allIds;
   }
-  return allIds;
 }
 
 void CoreDataset::addDataset(const seerep_core_msgs::DatasetIndexable& dataset)

seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp

@@ -46,6 +46,7 @@ seerep_core_msgs::Query CoreFbConversion::fromFb(const seerep::fb::Query* query,
   queryCore.polygon = fromFbQueryPolygon(query);
   queryCore.fullyEncapsulated = fromFbQueryFullyEncapsulated(query);
   queryCore.inMapFrame = fromFbQueryInMapFrame(query);
+  queryCore.sortByTime = query->sortByTime();
 
   return queryCore;
 }

seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp

@@ -19,6 +19,7 @@ seerep_core_msgs::Query CorePbConversion::fromPb(const seerep::pb::Query& query,
   fromFbQueryMaxNumData(query, queryCore);
   fromPbFullyEncapsulated(query, queryCore);
   fromPbInMapFrame(query, queryCore);
+  queryCore.sortByTime = query.sortbytime();
 
   return queryCore;
 }