Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feat/multi time query #416

Merged
merged 2 commits into from
Jan 28, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 12 additions & 4 deletions examples/python/gRPC/images/gRPC_fb_queryImages.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
createPoint2d,
createPolygon2D,
createQuery,
createTimeInterval,
createTimeIntervalVector,
createTimeStamp,
getOrCreateProject,
)
Expand Down Expand Up @@ -92,9 +92,16 @@ def query_images(

polygon_2d = createPolygon2D(fbb, 700, -100, vertices)

time_min = createTimeStamp(fbb, 1610549273, 0)
time_max = createTimeStamp(fbb, 1938549273, 0)
time_interval = createTimeInterval(fbb, time_min, time_max)
time_min = []
time_max = []

time_min.append(createTimeStamp(fbb, 1661336500, 0))
time_min.append(createTimeStamp(fbb, 1661336560, 0))

time_max.append(createTimeStamp(fbb, 1661336550, 0))
time_max.append(createTimeStamp(fbb, 1661336600, 0))

time_intervals = createTimeIntervalVector(fbb, time_min, time_max)

project_uuids = [project_uuid]

Expand All @@ -116,6 +123,7 @@ def query_images(
fbb,
grpc_channel,
project_uuid,
timeIntervalVector=time_intervals,
# polygon2d=polygon_2d,
polygon2dSensorPos=polygon_2d,
labels=labelsCategory,
Expand Down
11 changes: 7 additions & 4 deletions examples/python/gRPC/images/gRPC_pb_queryImage.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
from seerep.pb import meta_operations_pb2_grpc as metaOperations
from seerep.pb import point2d_pb2 as point2d
from seerep.pb import query_pb2 as query
from seerep.pb import time_interval_pb2 as timeInterval
from seerep.util.common import get_gRPC_channel


Expand Down Expand Up @@ -62,10 +63,12 @@ def query_images(
theQuery.polygon.vertices.extend(vertices)

# since epoche
theQuery.timeinterval.time_min.seconds = 1638549273
theQuery.timeinterval.time_min.nanos = 0
theQuery.timeinterval.time_max.seconds = 1938549273
theQuery.timeinterval.time_max.nanos = 0
time = timeInterval.TimeInterval()
time.time_min.seconds = 1638549273
time.time_min.nanos = 0
time.time_max.seconds = 1938549273
time.time_max.nanos = 0
theQuery.timeintervals.append(time)

# labels
labelsCategory = label_category_pb2.LabelCategory()
Expand Down
11 changes: 7 additions & 4 deletions examples/python/gRPC/images/gRPC_pb_queryImageGrid.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
from seerep.pb import meta_operations_pb2_grpc as metaOperations
from seerep.pb import point2d_pb2 as point2d
from seerep.pb import query_pb2 as query
from seerep.pb import time_interval_pb2 as timeInterval
from seerep.util.common import get_gRPC_channel

OFFSET = 0.5
Expand Down Expand Up @@ -42,10 +43,12 @@ def query_image_grid(
theQuery.projectuuid.append(target_project_uuid)

# since epoche
theQuery.timeinterval.time_min.seconds = 1638549273
theQuery.timeinterval.time_min.nanos = 0
theQuery.timeinterval.time_max.seconds = 1938549273
theQuery.timeinterval.time_max.nanos = 0
time = timeInterval.TimeInterval()
time.time_min.seconds = 1638549273
time.time_min.nanos = 0
time.time_max.seconds = 1938549273
time.time_max.nanos = 0
theQuery.timeintervals.append(time)

theQuery.polygon.z = -1
theQuery.polygon.height = 7
Expand Down
38 changes: 33 additions & 5 deletions examples/python/gRPC/util/fb_helper.py
Original file line number Diff line number Diff line change
Expand Up @@ -631,7 +631,7 @@ def addToPointFieldVector(builder: Builder, pointFieldList: List[int]) -> int:

def createQuery(
builder: Builder,
timeInterval: Union[int, None] = None,
timeIntervalVector: Union[int, None] = None,
labels: Union[List[int], None] = None,
mustHaveAllLabels: bool = False,
projectUuids: List[str] = None,
Expand All @@ -649,8 +649,9 @@ def createQuery(

Args:
builder: A flatbuffers Builder
timeInterval: The pointer to a TimeInterval object representing the time
frame of the returned instances labels: A list of pointers to\
timeIntervalVector: The pointer to vector of TimeInterval objects
representing the time frames
labels: A list of pointers to\
[LabelsWithInstanceWithCategory](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/labels_with_instance_with_category.fbs)\
flatbuffers objects, which the instances should at least have one of
mustHaveAllLabels: A boolean indicating if the returned instances should
Expand Down Expand Up @@ -716,8 +717,8 @@ def createQuery(
Query.AddPolygon(builder, polygon2d)
if polygon2dSensorPos:
Query.AddPolygonSensorPosition(builder, polygon2dSensorPos)
if timeInterval:
Query.AddTimeinterval(builder, timeInterval)
if timeIntervalVector:
Query.AddTimeintervals(builder, timeIntervalVector)
if labels:
Query.AddLabel(builder, labelOffset)
# no if; has default value
Expand Down Expand Up @@ -756,6 +757,33 @@ def createQueryInstance(builder: Builder, query: int, datatype: int) -> int:
return QueryInstance.End(builder)


def createTimeIntervalVector(
builder: Builder, timeMinList: List[int], timeMaxList: List[int]
) -> int:
"""
Create a vector of closed time intervals in flatbuffers.

Args:
builder: A flatbuffers Builder
timeMinList: The list of pointers to the Timestamp objects representing
the lower bound of the time of the intervals
timeMaxList: The list of pointers to the Timestamp objects representing
the upper bound of the time of the intervals

Returns:
A pointer to the constructed time interval vector object
"""
intervals = []
for timeMin, timeMax in zip(timeMinList, timeMaxList):
intervals.append(createTimeInterval(builder, timeMin, timeMax))

Query.StartTimeintervalsVector(builder, len(intervals))
for interval in intervals:
builder.PrependUOffsetTRelative(interval)

return builder.EndVector()


def createTimeInterval(builder: Builder, timeMin: int, timeMax: int) -> int:
"""
Create a closed time interval in flatbuffers.
Expand Down
3 changes: 2 additions & 1 deletion seerep_msgs/core/query.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@ struct Query
// polygon defined above
bool inMapFrame; // if false the query polygon is in geodetic coordinates,
// otherwise in map frame
std::optional<Timeinterval> timeinterval; ///< only do temporal query if set
std::optional<std::vector<Timeinterval>>
timeintervals; ///< only do temporal query if set
std::optional<std::unordered_map<std::string, std::vector<std::string>>>
label; ///< only do semantic query if set
std::optional<SparqlQuery>
Expand Down
2 changes: 1 addition & 1 deletion seerep_msgs/fbs/query.fbs
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ table Query {
polygonSensorPosition:Polygon2D;
fullyEncapsulated:bool;
inMapFrame:bool;
timeinterval:TimeInterval;
timeintervals:[TimeInterval];
label:[LabelCategory];
sparqlQuery:SparqlQuery;
ontologyURI:string;
Expand Down
2 changes: 1 addition & 1 deletion seerep_msgs/protos/query.proto
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ message Query
Polygon2D polygon = 1;
bool fullyEncapsulated = 2;
bool inMapFrame = 3;
TimeInterval timeinterval = 4;
repeated TimeInterval timeintervals = 4;
repeated LabelCategory labelCategory = 5;
bool mustHaveAllLabels = 6;
repeated string projectuuid = 7;
Expand Down
29 changes: 16 additions & 13 deletions seerep_srv/seerep_core/src/core_dataset.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ CoreDataset::getData(const seerep_core_msgs::Query& query)
}
}

if (!query.polygon && !query.polygonSensorPos && !query.timeinterval &&
if (!query.polygon && !query.polygonSensorPos && !query.timeintervals &&
!query.label && !query.instances && !query.dataUuids)
{
return getAllDatasetUuids(datatypeSpecifics, query.sortByTime);
Expand Down Expand Up @@ -335,21 +335,24 @@ std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>>
CoreDataset::queryTemporal(std::shared_ptr<DatatypeSpecifics> datatypeSpecifics,
const seerep_core_msgs::Query& query)
{
if (query.timeinterval)
if (query.timeintervals)
{
std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>> timetree_result =
std::vector<seerep_core_msgs::AabbTimeIdPair>();
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()));
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()));
}
return timetree_result;
}
else
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<seerep_core_msgs::Timeinterval>& queryCoreTime);
static void fromFbQueryTime(
const seerep::fb::Query* query,
std::optional<std::vector<seerep_core_msgs::Timeinterval>>& queryCoreTime);
/**
* @brief extracts the WithoutData Flag of the flatbuffer query message
* @param query the flatbuffer query message
Expand Down
30 changes: 16 additions & 14 deletions seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ CoreFbConversion::fromFb(const seerep::fb::Query* query,
seerep_core_msgs::Query queryCore;
queryCore.header.datatype = datatype;

fromFbQueryTime(query, queryCore.timeinterval);
fromFbQueryTime(query, queryCore.timeintervals);
fromFbQueryLabel(query, queryCore.label);
queryCore.sparqlQuery = fromFbSparqlQuery(query);
queryCore.ontologyURI = fromFbOntologyURI(query);
Expand Down Expand Up @@ -520,19 +520,21 @@ void CoreFbConversion::fromFbQueryLabel(

void CoreFbConversion::fromFbQueryTime(
const seerep::fb::Query* query,
std::optional<seerep_core_msgs::Timeinterval>& 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();
std::optional<std::vector<seerep_core_msgs::Timeinterval>>& queryCoreTimes)
{
if (flatbuffers::IsFieldPresent(query, seerep::fb::Query::VT_TIMEINTERVALS))
{
queryCoreTimes = std::vector<seerep_core_msgs::Timeinterval>();
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);
}
}
}

Expand Down
26 changes: 16 additions & 10 deletions seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -324,17 +324,23 @@ void CorePbConversion::fromPbLabel(const seerep::pb::Query& query,
void CorePbConversion::fromPbTime(const seerep::pb::Query& query,
seerep_core_msgs::Query& queryCore)
{
if (query.timeinterval().has_time_min() && query.timeinterval().has_time_max())
if (query.timeintervals_size() > 0)
{
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();
queryCore.timeintervals = std::vector<seerep_core_msgs::Timeinterval>();
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);
}
}
}
}

Expand Down
18 changes: 10 additions & 8 deletions seerep_srv/seerep_server/src/fb_image_service.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
<< ") /";
Expand All @@ -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()
<< ") /";
Expand All @@ -40,19 +40,21 @@ grpc::Status FbImageService::GetImage(
debuginfo << "bounding box height "
<< requestRoot->polygonSensorPosition()->height() << " /";
}
if (requestRoot->timeinterval() != NULL)
if (requestRoot->timeintervals() != NULL)
{
debuginfo << "\n time interval ("
<< requestRoot->timeinterval()->time_min()->seconds() << "/"
<< requestRoot->timeinterval()->time_max()->seconds() << ")";
for (auto&& timeinterval : *requestRoot->timeintervals())
{
debuginfo << "\n time interval (" << timeinterval->time_min()->seconds()
<< "/" << 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() << "' ";
}
Expand Down
Loading
Loading