diff --git a/examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py b/examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py index 64fec9390..63b67b887 100755 --- a/examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py +++ b/examples/python/gRPC/images/gRPC_fb_addCameraIntrinsics.py @@ -23,6 +23,7 @@ def add_camintrins_raw( ciuuid: Optional[str] = "fa2f27e3-7484-48b0-9f21-ec362075baca", target_proj_uuid: Optional[str] = None, grpc_channel: Channel = get_gRPC_channel(), + frame_id: str = "map", ) -> Optional[bytearray]: """ Creates a example cameraintrinsics object and sends it to a SEEREP server @@ -71,7 +72,7 @@ def add_camintrins_raw( # Create all necessary objects for the query ts = createTimeStamp(builder, 1, 2) - header = createHeader(builder, ts, "map", target_proj_uuid, ciuuid) + header = createHeader(builder, ts, frame_id, target_proj_uuid, ciuuid) roi = createRegionOfInterest(builder, 3, 5, 6, 7, True) distortion_matrix = [4, 5, 6, 7, 8, 9, 10, 11, 12] @@ -106,9 +107,10 @@ def add_camintrins( ciuuid: Optional[str] = "fa2f27e3-7484-48b0-9f21-ec362075baca", target_proj_uuid: Optional[str] = None, grpc_channel: Channel = get_gRPC_channel(), + frame_id: str = "map", ) -> Optional[CameraIntrinsics.CameraIntrinsics]: return CameraIntrinsics.CameraIntrinsics.GetRootAs( - add_camintrins_raw(ciuuid, target_proj_uuid, grpc_channel) + add_camintrins_raw(ciuuid, target_proj_uuid, grpc_channel, frame_id) ) diff --git a/examples/python/gRPC/images/gRPC_fb_addLabel.py b/examples/python/gRPC/images/gRPC_fb_addLabel.py index aa2414466..83a18a183 100755 --- a/examples/python/gRPC/images/gRPC_fb_addLabel.py +++ b/examples/python/gRPC/images/gRPC_fb_addLabel.py @@ -56,7 +56,7 @@ def add_label_raw( builder = flatbuffers.Builder(1024) query = createQuery( builder, - projectUuids=[builder.CreateString(target_proj_uuid)], + projectUuids=[target_proj_uuid], withoutData=True, ) builder.Finish(query) diff --git a/examples/python/gRPC/images/gRPC_fb_queryImages.py b/examples/python/gRPC/images/gRPC_fb_queryImages.py index 33ab080b5..ae601924d 100755 --- a/examples/python/gRPC/images/gRPC_fb_queryImages.py +++ b/examples/python/gRPC/images/gRPC_fb_queryImages.py @@ -43,11 +43,7 @@ def query_images_raw( """ image_service_stub = imageService.ImageServiceStub(grpc_channel) - project_uuid_buffer = fbb.CreateString(target_proj_uuid) - - query = createQuery( - fbb, *args, projectUuids=[project_uuid_buffer], **kwargs - ) + query = createQuery(fbb, *args, projectUuids=[target_proj_uuid], **kwargs) fbb.Finish(query) query_buf = fbb.Output() @@ -100,7 +96,7 @@ def query_images( time_max = createTimeStamp(fbb, 1938549273, 0) time_interval = createTimeInterval(fbb, time_min, time_max) - project_uuids = [fbb.CreateString(project_uuid)] + project_uuids = [project_uuid] labels = [ create_label(builder=fbb, label=label_str, label_id=i) diff --git a/examples/python/gRPC/images/gRPC_fb_sendImages.py b/examples/python/gRPC/images/gRPC_fb_sendImages.py index 7c16bea2c..c7dbc7a39 100644 --- a/examples/python/gRPC/images/gRPC_fb_sendImages.py +++ b/examples/python/gRPC/images/gRPC_fb_sendImages.py @@ -1,23 +1,31 @@ import random import time -from typing import List, Union +import uuid +from typing import Iterator, List, Tuple, Union from uuid import uuid4 import flatbuffers import numpy as np from grpc import Channel +from quaternion import quaternion +from seerep.fb import ServerResponse from seerep.fb import ( camera_intrinsics_service_grpc_fb as camera_intrinsic_service, ) from seerep.fb import image_service_grpc_fb as image_service +from seerep.fb import tf_service_grpc_fb as tf_service from seerep.util.common import get_gRPC_channel from seerep.util.fb_helper import ( createCameraIntrinsics, createHeader, createImage, createProject, + createQuaternion, createRegionOfInterest, createTimeStamp, + createTransform, + createTransformStamped, + createVector3, ) @@ -26,6 +34,8 @@ def send_images( project_uuid: str, camera_intrinsic_uuid: str, image_payloads: List[Union[np.ndarray, str]], + timestamps: Union[List[Tuple[int, int]], None] = None, + frame_id: str = "map", ) -> List[bytes]: """ Send images to a SEEREP project @@ -38,12 +48,21 @@ def send_images( image_payloads (List[Union[np.ndarray, str]]): A list of image payloads. Each payload can be either a numpy array (actual data) or a string (for a remote stroage URI). + timestamps (Union[List[Tuple[int, int]], None]): A list of timestamps + to attach to the images. Has to be the same size as image_payloads. Returns: List[bytes]: A list of bytes representing the serialized FlatBuffers messages. """ + if timestamps and len(timestamps) != len(image_payloads): + print( + "Couldn't send images, " + "due to len(image_payloads) != len(timestamps)" + ) + return [] + fbb = flatbuffers.Builder() image_service_stub = image_service.ImageServiceStub(grpc_channel) @@ -53,8 +72,17 @@ def send_images( ) fb_msgs = [] - for image in image_payloads: - header = createHeader(fbb, timestamp, "map", project_uuid, str(uuid4())) + for idx, image in enumerate(image_payloads): + if timestamps is None: + header = createHeader( + fbb, timestamp, frame_id, project_uuid, str(uuid4()) + ) + else: + timestamp = createTimeStamp(fbb, *timestamps[idx]) + header = createHeader( + fbb, timestamp, frame_id, project_uuid, str(uuid4()) + ) + fb_image = createImage( fbb, header, @@ -109,7 +137,9 @@ def create_project(grpc_channel: Channel, project_name: str) -> str: ) -def send_cameraintrinsics(grpc_channel: Channel, project_uuid: str) -> str: +def send_cameraintrinsics( + grpc_channel: Channel, project_uuid: str, frame_id: str = "map" +) -> str: """ Create and send a camera intrinsics object to SEEREP. @@ -126,7 +156,7 @@ def send_cameraintrinsics(grpc_channel: Channel, project_uuid: str) -> str: ) timestamp = createTimeStamp(fbb, 0, 0) ci_uuid = str(uuid4()) - header = createHeader(fbb, timestamp, "map", project_uuid, ci_uuid) + header = createHeader(fbb, timestamp, frame_id, project_uuid, ci_uuid) roi = createRegionOfInterest(fbb, 0, 0, 0, 0, False) distortion_matrix = [4, 5, 6, 7, 8, 9, 10, 11, 12] @@ -154,14 +184,82 @@ def send_cameraintrinsics(grpc_channel: Channel, project_uuid: str) -> str: return ci_uuid +def send_tfs( + grpc_channel: Channel, + project_uuid: str, + timestamps: List[Tuple[int, int]], + frame_id: str = "map", + child_frame_id: str = "camera", +) -> bytes: + """ + Stream len(timestamps) transforms with the form frame_id -> child_frame_id + to SEEREP. + + Args: + grpc_channel (Channel): The grpc_channel to a SEEREP server. + project_uuid (str): The project target for the transformations. + timestamps (List[Tuple[int, int]]): The timestamps of + the transformation. + frame_id (str): The parent frame for the transformations. + child_frame_id (str): The child frame for the transformations. + """ + + def tfs_gen() -> Iterator[bytes]: + builder = flatbuffers.Builder(1024) + quat = createQuaternion(builder, quaternion(1, 0, 0, 0)) + + for idx, (seconds, nanos) in enumerate(timestamps): + timestamp = createTimeStamp(builder, seconds, nanos) + + header = createHeader( + builder, timestamp, frame_id, project_uuid, str(uuid.uuid4()) + ) + + translation = createVector3( + builder, + np.array( + [100 * idx**2, 100 * idx**2, 30 * idx], dtype=np.float64 + ), + ) + + tf = createTransform(builder, translation, quat) + tfs = createTransformStamped(builder, child_frame_id, header, tf) + + builder.Finish(tfs) + yield bytes(builder.Output()) + + return tf_service.TfServiceStub(grpc_channel).TransferTransformStamped( + tfs_gen() + ) + + if __name__ == "__main__": channel = get_gRPC_channel() project_uuid = create_project(channel, "testproject") camera_uuid = send_cameraintrinsics(channel, project_uuid) + timestamp_nanos = 1245 + nanos_factor = 1e-9 + + timestamps = [ + (t, timestamp_nanos) for t in range(1661336507, 1661336608, 10) + ] + img_bufs = send_images( - channel, project_uuid, camera_uuid, generate_image_ressources(10) + channel, + project_uuid, + camera_uuid, + generate_image_ressources(10), + timestamps, ) + tf_resp = send_tfs(channel, project_uuid, timestamps) + if img_bufs is not None and len(img_bufs) > 0: print("Images sent successfully") + + if ( + ServerResponse.ServerResponse.GetRootAs(tf_resp).Message().decode() + == "everything stored!" + ): + print("Transforms sent successfully") diff --git a/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py b/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py index 124b2cd7d..33b9dd673 100755 --- a/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py +++ b/examples/python/gRPC/images/gRPC_pb_sendLabeledImageGrid.py @@ -3,7 +3,7 @@ import math import time import uuid -from typing import List, Tuple +from typing import List, Tuple, Union import flatbuffers import numpy as np @@ -33,7 +33,8 @@ def send_labeled_image_grid( - target_proj_uuid: str = None, grpc_channel: Channel = get_gRPC_channel() + target_proj_uuid: Union[str, None] = None, + grpc_channel: Channel = get_gRPC_channel(), ) -> Tuple[ List[List[image.Image]], List[Tuple[int, int]], diff --git a/examples/python/gRPC/instances/gRPC_fb_getInstances.py b/examples/python/gRPC/instances/gRPC_fb_getInstances.py index b4b567f53..7799d7ba6 100755 --- a/examples/python/gRPC/instances/gRPC_fb_getInstances.py +++ b/examples/python/gRPC/instances/gRPC_fb_getInstances.py @@ -47,7 +47,7 @@ timeInterval = createTimeInterval(builder, timeMin, timeMax) -projectUuids = [builder.CreateString(projectuuid)] +projectUuids = [projectuuid] labelStr = ["label1", "label2"] labels = [] @@ -63,8 +63,8 @@ ) ) -dataUuids = [builder.CreateString("5a0438b8-37cf-412e-8331-a95ef95c1016")] -instanceUuids = [builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248")] +dataUuids = ["5a0438b8-37cf-412e-8331-a95ef95c1016"] +instanceUuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] # 4. Create a query with parameters # All parameters are optional diff --git a/examples/python/gRPC/point/gRPC_fb_queryPoints.py b/examples/python/gRPC/point/gRPC_fb_queryPoints.py index 025601db8..d30c42120 100755 --- a/examples/python/gRPC/point/gRPC_fb_queryPoints.py +++ b/examples/python/gRPC/point/gRPC_fb_queryPoints.py @@ -44,7 +44,7 @@ def get_points_raw( timeMax = createTimeStamp(builder, 1938549273, 0) timeInterval = createTimeInterval(builder, timeMin, timeMax) - projectUuids = [builder.CreateString(target_proj_uuid)] + projectUuids = [target_proj_uuid] labelStr = ["label1", "label2"] labels = [] @@ -60,10 +60,8 @@ def get_points_raw( ) ) - dataUuids = [builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248")] - instanceUuids = [ - builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248") - ] + dataUuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] + instanceUuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] # 4. Create a query with parameters # all parameters are optional diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_addLabel.py b/examples/python/gRPC/pointcloud/gRPC_fb_addLabel.py index 01880c52a..0a8bf897a 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_addLabel.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_addLabel.py @@ -35,7 +35,7 @@ def add_pc_label_raw( query = createQuery( builder, - projectUuids=[builder.CreateString(target_proj_uuid)], + projectUuids=[target_proj_uuid], withoutData=True, ) builder.Finish(query) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index f5e4bd542..b2fcc637e 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -101,14 +101,10 @@ def query_pcs_raw( ) # filter for specific data - data_uuids = [ - fb_builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248") - ] - instance_uuids = [ - fb_builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248") - ] + data_uuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] + instance_uuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] - project_uuids = [fb_builder.CreateString(target_proj_uuid)] + project_uuids = [target_proj_uuid] # set the query parameters according to your needs query = createQuery( diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloudAndVisualize.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloudAndVisualize.py index 0c263db55..c016f0f92 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloudAndVisualize.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloudAndVisualize.py @@ -139,14 +139,10 @@ def query_pcs( ) # filter for specific data - data_uuids = [ - fb_builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248") - ] - instance_uuids = [ - fb_builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248") - ] + data_uuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] + instance_uuids = ["3e12e18d-2d53-40bc-a8af-c5cca3c3b248"] - project_uuids = [fb_builder.CreateString(target_proj_uuid)] + project_uuids = [target_proj_uuid] # set the query parameters according to your needs query = createQuery( diff --git a/examples/python/gRPC/simulated-data/querySimulatedData.py b/examples/python/gRPC/simulated-data/querySimulatedData.py index 08d4c1cae..9207aef8a 100755 --- a/examples/python/gRPC/simulated-data/querySimulatedData.py +++ b/examples/python/gRPC/simulated-data/querySimulatedData.py @@ -41,7 +41,7 @@ timeMax = createTimeStamp(builder, 1654688940, 0) timeInterval = createTimeInterval(builder, timeMin, timeMax) -projectUuids = [builder.CreateString(projectuuid)] +projectUuids = [projectuuid] labels = createLabelsWithCategoryVector( builder, ["ground_truth"], [["Gänseblümchen"]] ) diff --git a/examples/python/gRPC/tf/gRPC_fb_deleteTfs.py b/examples/python/gRPC/tf/gRPC_fb_deleteTfs.py new file mode 100755 index 000000000..0221b7226 --- /dev/null +++ b/examples/python/gRPC/tf/gRPC_fb_deleteTfs.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +import sys +import time +import uuid +from typing import Dict, List, Tuple, Union + +import flatbuffers +import numpy as np +from grpc import Channel +from quaternion import quaternion +from seerep.fb import TransformStampedIntervalQuery +from seerep.fb import tf_service_grpc_fb as tf_srv +from seerep.util.common import get_gRPC_channel +from seerep.util.fb_helper import ( + createHeader, + createQuaternion, + createTimeStamp, + createTransform, + createTransformStamped, + createTransformStampedQueryInterval, + createVector3, + getOrCreateProject, +) +from seerep.util.fb_to_dict import SchemaFileNames, fb_flatc_dict + + +# where time_min(inclusive) and time_max(exclusive) +# are of form Tuple[SECONDS, NANOSECONDS] +# and span the time interval to delete all tfs in between +def delete_tfs_raw( + time_min: Tuple[int, int], + time_max: Tuple[int, int], + frame_id: str = "map", + child_frame_id: str = "camera", + target_proj_uuid: Union[str, None] = None, + grpc_channel: Channel = get_gRPC_channel(), +) -> bytearray: + builder = flatbuffers.Builder(1024) + + if target_proj_uuid is None: + target_proj_uuid = getOrCreateProject( + flatbuffers.Builder(), grpc_channel, "testproject" + ) + + stubTf = tf_srv.TfServiceStub(grpc_channel) + + timestamp_min = createTimeStamp(builder, time_min[0], time_min[1]) + timestamp_max = createTimeStamp(builder, time_max[0], time_max[1]) + + cur_time_ns = time.time_ns() + cur_time_s = int(cur_time_ns // 1e9) + cur_timestamp = createTimeStamp( + builder, cur_time_s, int(cur_time_ns - cur_time_s * 1e9) + ) + + # timestamp in header is irrelevant, but does need to be filled in + header = createHeader(builder, cur_timestamp, frame_id, target_proj_uuid) + + tf_query_interval = createTransformStampedQueryInterval( + builder, header, child_frame_id, timestamp_min, timestamp_max + ) + builder.Finish(tf_query_interval) + buf = builder.Output() + + bufList = [bytes(buf)] + + stubTf.DeleteTransformStamped(iter(bufList)) + return buf + + +def delete_tfs( + time_min: Tuple[int, int], + time_max: Tuple[int, int], + frame_id: str = "map", + child_frame_id: str = "camera", + target_proj_uuid: Union[str, None] = None, + grpc_channel: Channel = get_gRPC_channel(), +) -> TransformStampedIntervalQuery.TransformStampedIntervalQuery: + buf = delete_tfs_raw( + time_min, + time_max, + frame_id, + child_frame_id, + target_proj_uuid, + grpc_channel, + ) + return ( + TransformStampedIntervalQuery.TransformStampedIntervalQuery.GetRootAs( + buf + ) + ) + + +def send_artificial_tfs( + grpc_channel: Channel, + project_uuid: str, + time_lst: List[Tuple[int, int]], + frame_id: str, + child_frame_id: str, +) -> List[Dict]: + # first send tfs with artifical timestamps + stub_tf = tf_srv.TfServiceStub(grpc_channel) + builder = flatbuffers.Builder(1024) + quat = createQuaternion(builder, quaternion(1, 0, 0, 0)) + + tf_list = [] + sent_tfs_base: List[Dict] = [] + for idx, time_tuple in enumerate(time_lst): + timestmp = createTimeStamp(builder, time_tuple[0], time_tuple[1]) + + # when not giving a uuid the corresponding retrieved tf from server has + # no uuid msg + header = createHeader( + builder, timestmp, frame_id, project_uuid, str(uuid.uuid4()) + ) + + translation = createVector3( + builder, + np.array([300 * idx, 300 * idx, 30 * idx], dtype=np.float64), + ) + + tf = createTransform(builder, translation, quat) + tf_s = createTransformStamped(builder, child_frame_id, header, tf) + + builder.Finish(tf_s) + buf = builder.Output() + sent_tfs_base.append( + fb_flatc_dict(buf, SchemaFileNames.TRANSFORM_STAMPED) + ) + tf_list.append(bytes(buf)) + + stub_tf.TransferTransformStamped(iter(tf_list)) + return sent_tfs_base + + +if __name__ == "__main__": + timestamp_nanos = 1245 + nanos_factor = 1e-9 + + timestamps = [ + (t, timestamp_nanos) for t in range(1661336507, 1661336558, 10) + ] + + grpc_channel = get_gRPC_channel() + + proj_uuid = getOrCreateProject( + flatbuffers.Builder(), grpc_channel, "testproject" + ) + + if grpc_channel is None or proj_uuid is None: + print("There is no project on the server or server is not reachable") + sys.exit() + + # sent_tfs = send_artificial_tfs( + # grpc_channel, proj_uuid, timestamps[:-2], "map", "camera" + # ) + del_tfs = delete_tfs( + timestamps[2], timestamps[3], "map", "camera", proj_uuid, grpc_channel + ) + print("deleted tfs") diff --git a/examples/python/gRPC/tf/gRPC_fb_getTf.py b/examples/python/gRPC/tf/gRPC_fb_getTf.py index d5e25001d..df702ea84 100755 --- a/examples/python/gRPC/tf/gRPC_fb_getTf.py +++ b/examples/python/gRPC/tf/gRPC_fb_getTf.py @@ -56,9 +56,7 @@ def get_tfs_raw( timestamp = createTimeStamp(builder, time_sec, time_nano) header = createHeader(builder, timestamp, frame, target_proj_uuid) - child_frame_id = builder.CreateString("camera") - - tf_query = createTransformStampedQuery(builder, header, child_frame_id) + tf_query = createTransformStampedQuery(builder, header, "camera") builder.Finish(tf_query) tf_buf: bytearray = stubTf.GetTransformStamped(bytes(builder.Output())) diff --git a/examples/python/gRPC/util/fb_helper.py b/examples/python/gRPC/util/fb_helper.py index 6680b251b..a83b3a2af 100644 --- a/examples/python/gRPC/util/fb_helper.py +++ b/examples/python/gRPC/util/fb_helper.py @@ -31,6 +31,7 @@ Timestamp, Transform, TransformStamped, + TransformStampedIntervalQuery, TransformStampedQuery, UuidDatatypePair, UuidDatatypeWithCategory, @@ -670,20 +671,32 @@ def createQuery( """ if projectUuids: - Query.StartProjectuuidVector(builder, len(projectUuids)) - for projectUuid in reversed(projectUuids): + # serialize strings + projectUuidsSerialized = [ + builder.CreateString(projectUuid) for projectUuid in projectUuids + ] + Query.StartProjectuuidVector(builder, len(projectUuidsSerialized)) + for projectUuid in reversed(projectUuidsSerialized): builder.PrependUOffsetTRelative(projectUuid) projectUuidsOffset = builder.EndVector() if instanceUuids: - Query.StartInstanceuuidVector(builder, len(instanceUuids)) - for instance in reversed(instanceUuids): + # serialize strings + instanceUuidsSerialized = [ + builder.CreateString(instanceUuid) for instanceUuid in instanceUuids + ] + Query.StartInstanceuuidVector(builder, len(instanceUuidsSerialized)) + for instance in reversed(instanceUuidsSerialized): builder.PrependUOffsetTRelative(instance) instanceOffset = builder.EndVector() if dataUuids: - Query.StartDatauuidVector(builder, len(dataUuids)) - for dataUuid in reversed(dataUuids): + # serialize strings + dataUuidsSerialized = [ + builder.CreateString(dataUuid) for dataUuid in dataUuids + ] + Query.StartDatauuidVector(builder, len(dataUuidsSerialized)) + for dataUuid in reversed(dataUuidsSerialized): builder.PrependUOffsetTRelative(dataUuid) dataUuidOffset = builder.EndVector() @@ -742,10 +755,10 @@ def createTimeInterval(builder: Builder, timeMin: int, timeMax: int) -> int: Args: builder: A flatbuffers Builder - timeMin: The pointer to a Timestamp object representing the lower bound - of the time of the interval - timeMax: The pointer to a Timestamp object representing the upper bound - of the time of the interval + timeMin: The pointer to a Timestamp object representing + the lower bound of the time of the interval + timeMax: The pointer to a Timestamp object representing + the upper bound of the time of the interval Returns: A pointer to the constructed time interval object @@ -770,12 +783,49 @@ def createTransformStampedQuery( Returns: A pointer to the constructed transform stamped query object """ + child_frame_obj = builder.CreateString(childFrameId) TransformStampedQuery.Start(builder) TransformStampedQuery.AddHeader(builder, header) - TransformStampedQuery.AddChildFrameId(builder, childFrameId) + TransformStampedQuery.AddChildFrameId(builder, child_frame_obj) return TransformStampedQuery.End(builder) +def createTransformStampedQueryInterval( + builder: Builder, + header: int, + childFrameId: str, + time_min: int, + time_max: int, +) -> int: + """ + Create a transform stamped query interval object in flatbuffers. + + Args: + builder: A flatbuffers Builder + header: The pointer to a header object including the parent frame id + childFrameId: The child frame id + time_min: the pointer to the earliest inclusive time stamp + flatbuffers object to span the time_interval + time_max: the pointer to the latest exclusive time stamp + flatbuffers object to span the time_interval + + Returns: + A pointer to the constructed transform stamped query interval object + """ + tf_stamped_query: int = createTransformStampedQuery( + builder, header, childFrameId + ) + time_interval: int = createTimeInterval(builder, time_min, time_max) + + TransformStampedIntervalQuery.Start(builder) + TransformStampedIntervalQuery.AddTransformStampedQuery( + builder, tf_stamped_query + ) + TransformStampedIntervalQuery.AddTimeInterval(builder, time_interval) + + return TransformStampedIntervalQuery.End(builder) + + def createRegionOfInterest( builder: Builder, x_offset: int, diff --git a/examples/python/gRPC/util/fb_to_dict.py b/examples/python/gRPC/util/fb_to_dict.py index b0a29e1ed..754342059 100644 --- a/examples/python/gRPC/util/fb_to_dict.py +++ b/examples/python/gRPC/util/fb_to_dict.py @@ -132,6 +132,8 @@ def fb_flatc_dict(fb_obj: bytearray, schema_file_name: SchemaFileNames) -> Dict: f"The schema file at {schema_path} does not exist!" ) + tmp_file = None + tmp_json = None try: with tempfile.NamedTemporaryFile(delete=False) as f: f.write(fb_obj) @@ -164,7 +166,9 @@ def fb_flatc_dict(fb_obj: bytearray, schema_file_name: SchemaFileNames) -> Dict: with open(tmp_json, "r") as tmp_f: json_dict = json.loads(tmp_f.read()) finally: - tmp_file.unlink(missing_ok=True) - tmp_json.unlink(missing_ok=True) + if tmp_file is not None: + tmp_file.unlink(missing_ok=True) + if tmp_json is not None: + tmp_json.unlink(missing_ok=True) return json_dict diff --git a/examples/python/gRPC/util/service_manager.py b/examples/python/gRPC/util/service_manager.py index a39fb52b2..6ad6c5379 100644 --- a/examples/python/gRPC/util/service_manager.py +++ b/examples/python/gRPC/util/service_manager.py @@ -1,4 +1,4 @@ -from typing import List +from typing import Dict, List from flatbuffers import Builder from grpc import Channel @@ -18,6 +18,7 @@ from seerep.fb import point_cloud_service_grpc_fb as pcl2_srv_fb from seerep.fb import point_service_grpc_fb as point_srv_fb from seerep.util.common import get_gRPC_channel +from seerep.util.fb_to_dict import SchemaFileNames, fb_flatc_dict class ServiceManager: @@ -80,14 +81,27 @@ def call_get_points_fb( point_lst.append(PointStamped.PointStamped.GetRootAs(response)) return point_lst - def call_get_images_fb( + def call_get_images_fb_raw( self, builder: Builder, query: Query.Query - ) -> List[Image.Image]: + ) -> List[bytes]: builder.Finish(query) buf = builder.Output() - queried_images = [] stub = self.get_stub(image_srv_fb.ImageServiceStub) - for response in stub.GetImage(bytes(buf)): + return stub.GetImage(bytes(buf)) + + def call_get_images_fb_dict( + self, builder: Builder, query: Query.Query + ) -> List[Dict]: + return [ + fb_flatc_dict(img, SchemaFileNames.IMAGE) + for img in self.call_get_images_fb_raw(builder, query) + ] + + def call_get_images_fb( + self, builder: Builder, query: Query.Query + ) -> List[Image.Image]: + queried_images = [] + for response in self.call_get_images_fb_raw(builder, query): queried_images.append(Image.Image.GetRootAs(response)) return queried_images diff --git a/seerep_com/README.md b/seerep_com/README.md index be51f8666..94dd2aeb0 100644 --- a/seerep_com/README.md +++ b/seerep_com/README.md @@ -56,6 +56,7 @@ server can send `1 ... N` messages during the service call. | Service | Description | Request Message | Response Message | Streaming | |------------------------|--------------------------------|----------------------------------------------------------------------------------------------------------|------------------|-----------| | [TransferTransformStamped](https://github.com/agri-gaia/seerep/blob/main/seerep_com/fbs/tf_service.fbs) | Add a transformation to SEEREP | [TransformStamped](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/transform_stamped.fbs) | [ServerResponse](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/server_response.fbs) | Client | +| [DeleteTransformStamped](https://github.com/agri-gaia/seerep/blob/main/seerep_com/fbs/tf_service.fbs) | Delete transformations from SEEREP | [TransformStampedIntervalQuery](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/transform_stamped_interval_query.fbs) | [ServerResponse](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/server_response.fbs) | Client | | [GetFrames](https://github.com/agri-gaia/seerep/blob/main/seerep_com/fbs/tf_service.fbs) | Get all frames of a project | [FrameQuery](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/frame_query.fbs) | [StringVector](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/string_vector.fbs) | - | | [GetTransformStamped](https://github.com/agri-gaia/seerep/blob/main/seerep_com/fbs/tf_service.fbs) | Get a transformation from SEEREP | [TransformStampedQuery](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/transform_stamped_query.fbs) | [TransformStamped](https://github.com/agri-gaia/seerep/blob/main/seerep_msgs/fbs/transform_stamped.fbs) | - | diff --git a/seerep_com/fbs/tf_service.fbs b/seerep_com/fbs/tf_service.fbs index 6e03da1e1..817df9454 100644 --- a/seerep_com/fbs/tf_service.fbs +++ b/seerep_com/fbs/tf_service.fbs @@ -1,6 +1,7 @@ include "transform_stamped.fbs"; include "transform_stamped_query.fbs"; +include "transform_stamped_interval_query.fbs"; include "server_response.fbs"; include "string_vector.fbs"; @@ -10,6 +11,7 @@ namespace seerep.fb; rpc_service TfService { TransferTransformStamped(seerep.fb.TransformStamped):seerep.fb.ServerResponse (streaming: "client"); + DeleteTransformStamped(seerep.fb.TransformStampedIntervalQuery):seerep.fb.ServerResponse (streaming: "client"); GetFrames(seerep.fb.FrameQuery):seerep.fb.StringVector; GetTransformStamped(seerep.fb.TransformStampedQuery):seerep.fb.TransformStamped; } diff --git a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_tf.h b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_tf.h index 4aa8c4654..30fa61bb6 100644 --- a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_tf.h +++ b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_tf.h @@ -26,6 +26,24 @@ class Hdf5FbTf : public Hdf5FbGeneral void writeTransformStamped(const seerep::fb::TransformStamped& tf); + /** + * @brief Deletes tf entries (time, translation, rotation) from hdf5. + * + * @param parentFrameId The parent frame of the the tf target. + * @param childFrameId The child frame of the the tf target. + * @param isStatic Whether the target tf is static or not. + * @param timeMin The inclusive timestamp to start the deletion from. + * @param timeMax The exclusive timestamp to end the deletion on. + * + * @return true when atleast one deletion took place. + * @return false when something went wrong during deletion, + * or no tf matched the parameters. + */ + bool deleteTransformStamped(std::string parentFrameId, + std::string childFrameId, const bool isStatic, + const seerep::fb::Timestamp& timeMin, + const seerep::fb::Timestamp& timeMax) const; + std::optional>> readTransformStamped(const std::string& id, const bool isStatic); std::optional> diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp index 6beedbd74..f1fce4e76 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp @@ -140,6 +140,181 @@ void Hdf5FbTf::writeTransformStamped(const seerep::fb::TransformStamped& tf) m_file->flush(); } +bool Hdf5FbTf::deleteTransformStamped(std::string parentFrameId, + std::string childFrameId, + const bool isStatic, + const seerep::fb::Timestamp& timeMin, + const seerep::fb::Timestamp& timeMax) const +{ + const std::scoped_lock lock(*m_write_mtx); + + std::string hdf5_group_tf; + if (isStatic) + { + hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF_STATIC; + } + else + { + hdf5_group_tf = seerep_hdf5_core::Hdf5CoreTf::HDF5_GROUP_TF; + } + + std::replace(parentFrameId.begin(), parentFrameId.end(), '/', '_'); + std::replace(childFrameId.begin(), childFrameId.end(), '/', '_'); + + std::string hdf5GroupPath = + hdf5_group_tf + "/" + parentFrameId + "_" + childFrameId; + std::string hdf5DatasetTimePath = hdf5GroupPath + "/" + "time"; + std::string hdf5DatasetTransPath = hdf5GroupPath + "/" + "translation"; + std::string hdf5DatasetRotPath = hdf5GroupPath + "/" + "rotation"; + + if (!m_file->exist(hdf5GroupPath) || !m_file->exist(hdf5DatasetTimePath) || + !m_file->exist(hdf5DatasetTransPath) || + !m_file->exist(hdf5DatasetRotPath)) + { + auto str_static = isStatic ? "static" : "non-static"; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "No " << str_static << " tfs matching the frames (" << parentFrameId + << " -> " << childFrameId << ") were found!"; + return false; + } + + std::shared_ptr group_ptr = + std::make_shared(m_file->getGroup(hdf5GroupPath)); + + // read size + if (!group_ptr->hasAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE)) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) + << "Could not access size attribute of the tf group " << parentFrameId + << "_" << childFrameId << "!"; + return false; + } + + long unsigned int size; + group_ptr->getAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE).read(size); + + if (size == 0) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "tf data has size 0."; + return false; + } + + auto data_set_time_ptr = std::make_shared( + m_file->getDataSet(hdf5DatasetTimePath)); + auto data_set_trans_ptr = std::make_shared( + m_file->getDataSet(hdf5DatasetTransPath)); + auto data_set_rot_ptr = std::make_shared( + m_file->getDataSet(hdf5DatasetRotPath)); + + std::vector> time; + data_set_time_ptr->read(time); + + std::vector> trans; + data_set_trans_ptr->read(trans); + + std::vector> rot; + data_set_rot_ptr->read(rot); + + // check if all have the right size + if (time.size() != size || trans.size() != size || rot.size() != size) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "sizes of time (" << time.size() << "), translation (" + << trans.size() << ") and rotation (" << rot.size() + << ") not matching. Size expected by value in metadata (" << size + << ")"; + return false; + } + + // sort indices by time + // initialize original index locations + std::vector idx(time.size()); + iota(idx.begin(), idx.end(), 0); + + // sort indexes based on comparing values in window + // using std::stable_sort instead of std::sort + // to avoid unnecessary index re-orderings + // when window contains elements of equal values + stable_sort(idx.begin(), idx.end(), [&time](size_t i1, size_t i2) { + return time[i1][0] < time[i2][0] || + (time[i1][0] == time[i2][0] && time[i1][1] < time[i2][1]); + }); + + // inclusive first index search + auto timeMinIt = + std::find_if(idx.begin(), idx.end(), [&time, &timeMin](size_t i) { + return time[i][0] > timeMin.seconds() || + (time[i][0] == timeMin.seconds() && + time[i][1] >= timeMin.nanos()); + }); + + // exclusive last index search + auto timeMaxIt = + std::find_if(idx.begin(), idx.end(), [&time, &timeMax](size_t i) { + return time[i][0] > timeMax.seconds() || + (time[i][0] == timeMax.seconds() && + time[i][1] >= timeMax.nanos()); + }); + + // first inclusive index + auto firstDelIdx = std::distance(idx.begin(), timeMinIt); + // last exclusive index + auto endDelIdx = std::distance(idx.begin(), timeMaxIt); + + // early abort if no transform is between the provided time interval + if (timeMinIt == idx.end() || firstDelIdx >= endDelIdx) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "No tf was found in the timeinterval between " << timeMin.seconds() + << "s / " << timeMin.nanos() << "ns (inclusive) and " + << timeMax.seconds() << "s / " << timeMax.nanos() << "ns (exclusive)!"; + return false; + } + std::vector> reduced_time; + std::vector> reduced_trans; + std::vector> reduced_rot; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) + << "Deleting tfs from " << timeMin.seconds() << "s / " << timeMin.nanos() + << "ns (inclusive) up to " << timeMax.seconds() << "s / " + << timeMax.nanos() << "ns (exclusive)..."; + + for (size_t i = 0; i < idx.size(); ++i) + { + // ignore all elements in the timeinterval + if (i == (size_t)firstDelIdx) + { + if ((size_t)endDelIdx == idx.size()) + { + break; + } + i = endDelIdx; + } + reduced_time.push_back(time[idx[i]]); + reduced_trans.push_back(trans[idx[i]]); + reduced_rot.push_back(rot[idx[i]]); + } + + auto reduced_size = reduced_time.size(); + + data_set_time_ptr->resize({ reduced_size, 2 }); + data_set_time_ptr->select({ 0, 0 }, { reduced_size, 2 }).write(reduced_time); + + data_set_trans_ptr->resize({ reduced_size, 3 }); + data_set_trans_ptr->select({ 0, 0 }, { reduced_size, 3 }).write(reduced_trans); + + data_set_rot_ptr->resize({ reduced_size, 4 }); + data_set_rot_ptr->select({ 0, 0 }, { reduced_size, 4 }).write(reduced_rot); + + // write the size as group attribute + group_ptr->getAttribute(seerep_hdf5_core::Hdf5CoreTf::SIZE).write(reduced_size); + + m_file->flush(); + return true; +} + std::optional>> Hdf5FbTf::readTransformStamped(const std::string& id, const bool isStatic) { diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 9ccde26bd..07edf9088 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -82,6 +82,7 @@ set(MY_FBS_FILES fbs/time_interval.fbs fbs/timestamp.fbs fbs/transform_stamped.fbs + fbs/transform_stamped_interval_query.fbs fbs/transform_stamped_query.fbs fbs/transform.fbs fbs/union_map_entry.fbs diff --git a/seerep_msgs/fbs/transform_stamped_interval_query.fbs b/seerep_msgs/fbs/transform_stamped_interval_query.fbs new file mode 100644 index 000000000..be58023d5 --- /dev/null +++ b/seerep_msgs/fbs/transform_stamped_interval_query.fbs @@ -0,0 +1,12 @@ +include "transform_stamped_query.fbs"; +include "time_interval.fbs"; + +namespace seerep.fb; + +table TransformStampedIntervalQuery +{ + transform_stamped_query: seerep.fb.TransformStampedQuery; + time_interval: seerep.fb.TimeInterval; +} + +root_type TransformStampedIntervalQuery; diff --git a/seerep_srv/seerep_core/include/seerep_core/core.h b/seerep_srv/seerep_core/include/seerep_core/core.h index 9164b16fb..51147be90 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core.h +++ b/seerep_srv/seerep_core/include/seerep_core/core.h @@ -109,6 +109,12 @@ class Core */ std::vector getFrames(const boost::uuids::uuid& projectuuid); + /** + * @brief Recreates the TF buffer from hdf5 + * @param projectuuid the UUID of the targeted project + */ + void reinitializeTFs(const boost::uuids::uuid& projectuuid); + /** * @brief Adds a ci to the project * @param ci the Camera Intrinsics to be added 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 9c2cdb6cb..b8da3430d 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -99,6 +99,16 @@ class CoreDataset const seerep_core_msgs::Datatype& datatype, std::shared_ptr hdf5Io); + /** + * @brief Recreate the spatial rtree + * + * @param datatype the datatype for which to recreate the spatial rtree + * @param hdf5Io pointer to the object handling HDF5 io for this datatype + */ + void recreateSpatialRt( + const seerep_core_msgs::Datatype& datatype, + std::shared_ptr hdf5Io); + /** * @brief Returns a vector of UUIDs of datasets that match the query * @param query the spatio-temporal-semantic query @@ -209,10 +219,20 @@ class CoreDataset */ void tryAddingDataWithMissingTF(const seerep_core_msgs::Datatype& datatype); + /** + * @brief Checks the canTransform on the tf buffer for the indexable + * + * @param indexable the abstract indexable + * @return true when a transform for the indexable dataset is in the tf buffer + * @return false else + */ + bool + isSpatiallyTransformable(const seerep_core_msgs::DatasetIndexable& indexable); + /** * @brief Check if the created CGAL polygon follows the requirements. It - * should be simple (no more than two vertices on an edge), convex (no inward - * egdes), the vertices should be in a counter clockwise order. + * should be simple (no more than two vertices on an edge), convex (no + * inward egdes), the vertices should be in a counter clockwise order. * * @param polygon_cgal a polygon defined with CGAL * @return true The polygon abides by CGAL requirements @@ -220,6 +240,16 @@ class CoreDataset */ bool verifyPolygonIntegrity(CGAL::Polygon_2& polygon_cgal); + /** + * @brief transforms the bounding box to the datasets frameId (mostly the map + * frame) + * + * @param indexable the indexable to transform + * @return The transformed AABB + */ + seerep_core_msgs::AABB + transformIndexableAABB(const seerep_core_msgs::DatasetIndexable& indexable); + /** * @brief convert core msg polygon to CGAL polygon * 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 54266caff..6b2ec3af9 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_project.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_project.h @@ -156,6 +156,12 @@ class CoreProject * @brief Returns a vector of all frames stored in the TF tree by the TF buffer * @return vector of frame names */ + std::vector getFrames(); + + /** + * @brief reinitializes the tf buffer from hdf5 + */ + void reinitializeTFs(); // camera intrinsics /** @@ -184,8 +190,6 @@ class CoreProject */ bool cameraIntrinsicExists(boost::uuids::uuid camIntrinsicsUuid); - std::vector getFrames(); - /** * @brief Returns a shared pointer to the file mutex of the HDF5 file of this project * @return shared pointer to the HDF5 file mutex diff --git a/seerep_srv/seerep_core/include/seerep_core/core_tf.h b/seerep_srv/seerep_core/include/seerep_core/core_tf.h index 0a0a88f1c..48871c595 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_tf.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_tf.h @@ -97,6 +97,12 @@ class CoreTf */ std::vector getFrames(); + /** + * @brief cleans the current tf buffer and loads the TFs into the buffer from + * the HDF5 file + */ + void recreateBufferAndDatasets(); + private: /** * @brief loads the TFs into the buffer from the HDF5 file diff --git a/seerep_srv/seerep_core/src/core.cpp b/seerep_srv/seerep_core/src/core.cpp index 83523b1d9..58827cdb2 100644 --- a/seerep_srv/seerep_core/src/core.cpp +++ b/seerep_srv/seerep_core/src/core.cpp @@ -196,6 +196,12 @@ void Core::addTF(const geometry_msgs::TransformStamped& tf, project->second->addTF(tf); } +void Core::reinitializeTFs(const boost::uuids::uuid& projectuuid) +{ + auto project = findProject(projectuuid); + project->second->reinitializeTFs(); +} + void Core::addCameraIntrinsics(const seerep_core_msgs::camera_intrinsics& ci, const boost::uuids::uuid& projectuuid) { diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index b30bcae87..0c5f26a20 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -63,6 +63,48 @@ void CoreDataset::recreateDatasets( } } +void CoreDataset::recreateSpatialRt( + const seerep_core_msgs::Datatype& datatype, + std::shared_ptr hdf5Io) +{ + std::vector datasets = hdf5Io->getDatasetUuids(); + auto datatypeSpecifics = m_datatypeDatatypeSpecificsMap.at(datatype); + std::vector insertableData; + + for (auto uuid : datasets) + { + try + { + std::optional dataset = + hdf5Io->readDataset(uuid); + + if (dataset) + { + if (isSpatiallyTransformable(dataset.value())) + { + insertableData.push_back( + { transformIndexableAABB(dataset.value()), + boost::lexical_cast(uuid) }); + } + else + { + datatypeSpecifics->dataWithMissingTF.push_back( + std::make_shared( + dataset.value())); + } + } + } + catch (const std::runtime_error& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) + << e.what(); + } + } + // recreate spatial rtree + datatypeSpecifics->rt = + seerep_core_msgs::rtree{ insertableData.begin(), insertableData.end() }; +} + std::vector CoreDataset::getData(const seerep_core_msgs::Query& query) { @@ -221,19 +263,20 @@ CoreDataset::querySpatial(std::shared_ptr datatypeSpecifics, // traverse query results and confirm if they are contained inside the polygon for (it = rt_result.value().begin(); it != rt_result.value().end();) { - // check if query result is not fully distant to the oriented bb box provided in the query + // check if query result is not fully distant to the oriented bb box + // provided in the query intersectionDegree(it->first, query.polygon.value(), fullyEncapsulated, partiallyEncapsulated); - // if there is no intersection between the result and the user's request, - // remove it from the iterator + // if there is no intersection between the result and the user's + // request, remove it from the iterator if (!partiallyEncapsulated && !fullyEncapsulated) { it = rt_result.value().erase(it); } - // does the user want only results fully encapsulated by the requested bb - // and is this query result not fully encapsulated + // does the user want only results fully encapsulated by the requested + // bb and is this query result not fully encapsulated else if (query.fullyEncapsulated && !fullyEncapsulated) { // if yes, then delete partially encapsulated query results from the result set @@ -595,15 +638,10 @@ void CoreDataset::tryAddingDataWithMissingTF( it != datatypeSpecifics->dataWithMissingTF.end(); /*it++*/ /*<-- increment in loop itself!*/) { - if (m_tfOverview->canTransform((*it)->header.frameId, m_frameId, - (*it)->header.timestamp.seconds, - (*it)->header.timestamp.nanos)) + if (isSpatiallyTransformable(**it)) { - datatypeSpecifics->rt.insert(std::make_pair( - m_tfOverview->transformAABB( - (*it)->boundingbox, (*it)->header.frameId, m_frameId, - (*it)->header.timestamp.seconds, (*it)->header.timestamp.nanos), - (*it)->header.uuidData)); + datatypeSpecifics->rt.insert( + std::make_pair(transformIndexableAABB(**it), (*it)->header.uuidData)); it = datatypeSpecifics->dataWithMissingTF.erase(it); } else @@ -619,15 +657,10 @@ void CoreDataset::addDatasetToIndices( { auto datatypeSpecifics = m_datatypeDatatypeSpecificsMap.at(datatype); - if (m_tfOverview->canTransform(dataset.header.frameId, m_frameId, - dataset.header.timestamp.seconds, - dataset.header.timestamp.nanos)) + if (isSpatiallyTransformable(dataset)) { - datatypeSpecifics->rt.insert(std::make_pair( - m_tfOverview->transformAABB(dataset.boundingbox, dataset.header.frameId, - m_frameId, dataset.header.timestamp.seconds, - dataset.header.timestamp.nanos), - dataset.header.uuidData)); + datatypeSpecifics->rt.insert(std::make_pair(transformIndexableAABB(dataset), + dataset.header.uuidData)); } else { @@ -796,8 +829,8 @@ void CoreDataset::intersectionDegree(const seerep_core_msgs::AABB& aabb, // convert seerep core polygon to cgal polygon CGAL::Polygon_2 polygon_cgal = toCGALPolygon(polygon); - // a cgal polyon needs to be simple, convex and the points should be added in - // a counter clockwise order + // a cgal polyon needs to be simple, convex and the points should be added + // in a counter clockwise order if (!verifyPolygonIntegrity(polygon_cgal) || !verifyPolygonIntegrity(aabb_cgal)) { @@ -972,4 +1005,20 @@ CoreDataset::getAllLabels(std::vector datatypes, return labels; } +bool CoreDataset::isSpatiallyTransformable( + const seerep_core_msgs::DatasetIndexable& dataset) +{ + return m_tfOverview->canTransform(dataset.header.frameId, m_frameId, + dataset.header.timestamp.seconds, + dataset.header.timestamp.nanos); +} + +seerep_core_msgs::AABB CoreDataset::transformIndexableAABB( + const seerep_core_msgs::DatasetIndexable& indexable) +{ + return m_tfOverview->transformAABB(indexable.boundingbox, + indexable.header.frameId, m_frameId, + indexable.header.timestamp.seconds, + indexable.header.timestamp.nanos); +} } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index 188805597..38a138cdb 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -173,6 +173,17 @@ std::vector CoreProject::getFrames() return m_coreTfs->getFrames(); } +void CoreProject::reinitializeTFs() +{ + m_coreTfs->recreateBufferAndDatasets(); + m_coreDatasets->recreateSpatialRt(seerep_core_msgs::Datatype::Image, + m_ioImage); + m_coreDatasets->recreateSpatialRt(seerep_core_msgs::Datatype::PointCloud, + m_ioPointCloud); + m_coreDatasets->recreateSpatialRt(seerep_core_msgs::Datatype::Point, + m_ioPoint); +} + void CoreProject::addCameraIntrinsics( const seerep_core_msgs::camera_intrinsics& ci) { diff --git a/seerep_srv/seerep_core/src/core_tf.cpp b/seerep_srv/seerep_core/src/core_tf.cpp index e9707596a..2461c6284 100644 --- a/seerep_srv/seerep_core/src/core_tf.cpp +++ b/seerep_srv/seerep_core/src/core_tf.cpp @@ -23,6 +23,13 @@ void CoreTf::recreateDatasets() loadTfs(tfs_static, true); } +void CoreTf::recreateBufferAndDatasets() +{ + // cannot recreate the tf buffer object, because its protected by a mutex + m_tfBuffer.clear(); + this->recreateDatasets(); +} + void CoreTf::loadTfs(std::vector tfs, const bool isStatic) { for (auto const& name : tfs) diff --git a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_tf.h b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_tf.h index 947f40501..75d6e4294 100644 --- a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_tf.h +++ b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_tf.h @@ -10,6 +10,7 @@ // seerep-msgs #include +#include #include // seerep_core_msgs @@ -61,11 +62,31 @@ class CoreFbTf void addData(const seerep::fb::TransformStamped& tf); /** - * @brief gets all the frames which are in the tf tree of the project of interest + * @brief Delete a range of tfs based on specified time interval from hdf5. + * @param tfInterval types of tfs to delete in a given time. + * + * The tfs are deleted from hdf5 in the specified project. + * + * @return std::nullopt if no deletions were done. + * @return the projectuuid when atleast one deletion was executed. + */ + std::optional + deleteHdf5(const seerep::fb::TransformStampedIntervalQuery& tfInterval); + + /** + * @brief Recreate the tfBuffer from hdf5 for the specified project + * @param projectuuid uuid of the project to recreate the tfBuffer in + */ + void reinitializeTFs(const boost::uuids::uuid& projectuuid); + + /** + * @brief gets all the frames which are in the tf tree of the project of + * interest * @param projectuuid the uuid of the project of interest * @return a vector of all the frames in the tf tree of the project * - * The tf is stored in the hdf5 file via hdf5-io-fb. The tf is also added to the tf buffer. + * The tf is stored in the hdf5 file via hdf5-io-fb. The tf is also added + * to the tf buffer. */ std::vector getFrames(const boost::uuids::uuid& projectuuid); diff --git a/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp b/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp index 4db11b03d..4739870cb 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_tf.cpp @@ -54,4 +54,44 @@ CoreFbTf::getFrames(const boost::uuids::uuid& projectuuid) return m_seerepCore->getFrames(projectuuid); } +std::optional +CoreFbTf::deleteHdf5(const seerep::fb::TransformStampedIntervalQuery& tfInterval) +{ + auto projectuuid = boost::lexical_cast( + tfInterval.transform_stamped_query()->header()->uuid_project()->str()); + + // delete from hdf5 files + auto hdf5io = CoreFbGeneral::getHdf5(projectuuid, m_seerepCore, m_hdf5IoMap); + + // non static frames + bool wasDeleted = hdf5io->deleteTransformStamped( + tfInterval.transform_stamped_query()->header()->frame_id()->str(), + tfInterval.transform_stamped_query()->child_frame_id()->str(), false, + *tfInterval.time_interval()->time_min(), + *tfInterval.time_interval()->time_max()); + + // static frames + if (hdf5io->deleteTransformStamped( + tfInterval.transform_stamped_query()->header()->frame_id()->str(), + tfInterval.transform_stamped_query()->child_frame_id()->str(), true, + *tfInterval.time_interval()->time_min(), + *tfInterval.time_interval()->time_max())) + { + wasDeleted = true; + }; + + if (wasDeleted) + { + return projectuuid; + } + + return std::nullopt; +} + +void CoreFbTf::reinitializeTFs(const boost::uuids::uuid& projectuuid) +{ + // recreate the tf buffer + m_seerepCore->reinitializeTFs(projectuuid); +} + } // namespace seerep_core_fb diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_tf_service.h b/seerep_srv/seerep_server/include/seerep_server/fb_tf_service.h index 48db87060..bf7b97ebb 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_tf_service.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_tf_service.h @@ -24,6 +24,13 @@ class FbTfService final : public seerep::fb::TfService::Service grpc::ServerReader< flatbuffers::grpc::Message>* reader, flatbuffers::grpc::Message* response); + + grpc::Status DeleteTransformStamped( + grpc::ServerContext* context, + grpc::ServerReader>* reader, + flatbuffers::grpc::Message* response); + grpc::Status GetFrames(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, diff --git a/seerep_srv/seerep_server/src/fb_tf_service.cpp b/seerep_srv/seerep_server/src/fb_tf_service.cpp index d117b1172..cd2a2b2d2 100644 --- a/seerep_srv/seerep_server/src/fb_tf_service.cpp +++ b/seerep_srv/seerep_server/src/fb_tf_service.cpp @@ -78,6 +78,107 @@ grpc::Status FbTfService::TransferTransformStamped( return grpc::Status::OK; } +grpc::Status FbTfService::DeleteTransformStamped( + grpc::ServerContext* context, + grpc::ServerReader< + flatbuffers::grpc::Message>* + reader, + flatbuffers::grpc::Message* response) +{ + (void)context; // ignore that variable without causing warnings + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) + << "received transform deletion request... "; + + std::string answer = "selected tfs deleted!"; + + flatbuffers::grpc::Message + tfIntervalQueryMsg; + std::unordered_set> + projectUuids; + while (reader->Read(&tfIntervalQueryMsg)) + { + auto tfIntervalQuery = tfIntervalQueryMsg.GetRoot(); + auto projectUuidStr = tfIntervalQuery->transform_stamped_query() + ->header() + ->uuid_project() + ->str(); + if (!projectUuidStr.empty()) + { + try + { + if (auto deletedInProjectUuid = tfFb->deleteHdf5(*tfIntervalQuery)) + { + projectUuids.insert(deletedInProjectUuid.value()); + } + } + 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); + } + } + else + { + answer = "one msg had no project uuid!"; + } + } + for (auto projectUuid : projectUuids) + { + try + { + tfFb->reinitializeTFs(projectUuid); + } + catch (std::runtime_error const& e) + { + // mainly 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); + } + } + + seerep_server_util::createResponseFb( + answer, seerep::fb::TRANSMISSION_STATE_SUCCESS, response); + + return grpc::Status::OK; +} + grpc::Status FbTfService::GetFrames( grpc::ServerContext* context, const flatbuffers::grpc::Message* request, diff --git a/tests/python/gRPC/images/test_gRPC_fb_addLabels.py b/tests/python/gRPC/images/test_gRPC_fb_addLabels.py index d68e68d5b..73425768d 100644 --- a/tests/python/gRPC/images/test_gRPC_fb_addLabels.py +++ b/tests/python/gRPC/images/test_gRPC_fb_addLabels.py @@ -23,7 +23,7 @@ def get_imgs(target_proj_uuid: str, grpc_channel: Channel) -> List: stub = imageService.ImageServiceStub(grpc_channel) query = createQuery( builder, - projectUuids=[builder.CreateString(target_proj_uuid)], + projectUuids=[target_proj_uuid], withoutData=True, ) builder.Finish(query) diff --git a/tests/python/gRPC/instances/test_gRPC_fb_getInstances.py b/tests/python/gRPC/instances/test_gRPC_fb_getInstances.py index b6b66a238..de178b4c3 100644 --- a/tests/python/gRPC/instances/test_gRPC_fb_getInstances.py +++ b/tests/python/gRPC/instances/test_gRPC_fb_getInstances.py @@ -423,7 +423,7 @@ def test_gRPC_getInstanceQueryInstanceUuid(grpc_channel, project_setup): ) query_builder.set_active_function( EnumFbQuery.PROJECTUUID, - lambda: [query_builder.builder.CreateString(proj_uuid)], + lambda: [proj_uuid], ) query_builder.assemble_datatype_instance() @@ -479,7 +479,7 @@ def test_gRPC_getInstanceQueryInstanceUuid(grpc_channel, project_setup): ) query_builder.set_active_function( EnumFbQuery.PROJECTUUID, - lambda: [query_builder.builder.CreateString(proj_uuid)], + lambda: [proj_uuid], ) query_builder.assemble_datatype_instance() @@ -530,7 +530,7 @@ def test_gRPC_getInstanceQueryDatauuid(grpc_channel, project_setup): ) query_builder.set_active_function( EnumFbQuery.PROJECTUUID, - lambda: [query_builder.builder.CreateString(proj_uuid)], + lambda: [proj_uuid], ) query_builder.assemble_datatype_instance() diff --git a/tests/python/gRPC/tf/test_gRPC_fb_deleteTfs.py b/tests/python/gRPC/tf/test_gRPC_fb_deleteTfs.py new file mode 100644 index 000000000..5d80e58d8 --- /dev/null +++ b/tests/python/gRPC/tf/test_gRPC_fb_deleteTfs.py @@ -0,0 +1,141 @@ +# test file for +# gRPC_fb_getTf.py +from typing import Dict, List + +import flatbuffers +from gRPC.images import gRPC_fb_sendImages as send_img +from gRPC.tf import gRPC_fb_deleteTfs as del_tfs +from gRPC.util.msg_abs.msgs import ( + DatatypeImplementations, + EnumFbQuery, + FbQuery, + ServiceManager, +) + + +def sort_call(serv_man: ServiceManager, query: FbQuery) -> List[Dict]: + return sorted( + serv_man.call_get_images_fb_dict( + query.builder, query.datatype_instance + ), + key=lambda x: x["header"]["stamp"]["seconds"], + ) + + +def change_assemble_call( + serv_man: ServiceManager, query: FbQuery, extent: float +) -> List[Dict]: + query.set_active_function( + EnumFbQuery.POLYGON, + lambda: DatatypeImplementations.Fb.parameterized_polygon2d( + query.builder, extent, 500 + ), + ) + + query.assemble_datatype_instance() + return sort_call(serv_man, query) + + +# test sending images with tfs and deleting the tfs in part +def test_gRPC_fb_deleteTfs(grpc_channel, project_setup): + _, project_uuid = project_setup + timestamp_nanos = 1245 + timestamps = [ + (t, timestamp_nanos) for t in range(1661336507, 1661336608, 10) + ] + + camera_uuid = send_img.send_cameraintrinsics(grpc_channel, project_uuid) + + # per timestamp the tf is increased by 100 times according + # to the index of the timestamp + send_img.send_images( + grpc_channel, + project_uuid, + camera_uuid, + send_img.generate_image_ressources(10), + timestamps, + ) + + send_img.send_tfs(grpc_channel, project_uuid, timestamps) + + builder = flatbuffers.Builder() + + # first check what happens if the first tfs are deleted + # for testing execute a image query on that area + query = FbQuery( + grpc_channel, + builder, + {EnumFbQuery.WITHOUTDATA, EnumFbQuery.POLYGON, EnumFbQuery.PROJECTUUID}, + ) + + query.set_active_function(EnumFbQuery.PROJECTUUID, lambda: [project_uuid]) + + serv_man = ServiceManager(grpc_channel) + + # this value should stay this way, otherwise the other functions + # have to be altered as well + ts_idx = 3 + + images_before = change_assemble_call(serv_man, query, 400) + + # the first 2 images shouldn't have a valid transform now + del_tfs.delete_tfs( + timestamps[0], + timestamps[ts_idx], + "map", + "camera", + project_uuid, + grpc_channel, + ) + + images_after = sort_call(serv_man, query) + + assert images_after == images_before[ts_idx:] + + # if tfs in between are deleted, tfs are interpolated linearly + # check the first 6 images (now 4 because of the deletion before) + images_before = change_assemble_call(serv_man, query, 1600) + + # tfs at 1600 and 2500 are deleted, the same images + # must occur at (900+900) and (900+1800) [900, 3600] + del_tfs.delete_tfs( + timestamps[ts_idx + 1], + timestamps[ts_idx + 3], + "map", + "camera", + project_uuid, + grpc_channel, + ) + + images_after = sort_call(serv_man, query) + assert images_after == images_before[:-1] + + images_after = change_assemble_call(serv_man, query, 1798) + assert images_after == images_before[0:1] + + images_after = change_assemble_call(serv_man, query, 1800) + assert images_after == images_before + + images_before = change_assemble_call(serv_man, query, 3600) + + images_after = change_assemble_call(serv_man, query, 2698) + assert images_after == images_before[0:2] + + images_after = change_assemble_call(serv_man, query, 2700) + assert images_after == images_before[0:3] + + images_before = change_assemble_call(serv_man, query, 100 * 10**2) + + # test what happens with the end edge case + del_tfs.delete_tfs( + timestamps[-1], + (timestamps[-1][0] + 1, timestamps[-1][1]), + "map", + "camera", + project_uuid, + grpc_channel, + ) + + # the last image shouldn't have a valid tf + images_after = sort_call(serv_man, query) + assert images_after == images_before[:-1] diff --git a/tests/python/gRPC/util/msg_abs/msgs.py b/tests/python/gRPC/util/msg_abs/msgs.py index 6a3cbb8c6..88d93892e 100644 --- a/tests/python/gRPC/util/msg_abs/msgs.py +++ b/tests/python/gRPC/util/msg_abs/msgs.py @@ -50,7 +50,7 @@ class FbQuery(MsgsFb[Query.Query]): def _set_enum_func_mapping(self) -> Dict[EnumFbQuery, MsgsFunctions]: return { EnumFbQuery.POLYGON: MsgsFunctions( - lambda: None, lambda: Dtypes.Fb.polygon2D(self.builder) + lambda: None, lambda: Dtypes.Fb.polygon2d(self.builder) ), EnumFbQuery.FULLY_ENCAPSULATED: MsgsFunctions( lambda: False, lambda: True @@ -172,7 +172,7 @@ def _assemble_datatype_instance(self): class DatatypeImplementations: class Fb: @classmethod - def polygon2D(cls, builder: Builder) -> Polygon2D.Polygon2D: + def polygon2d(cls, builder: Builder) -> Polygon2D.Polygon2D: polygon_vertices = [] polygon_vertices.append(fbh.createPoint2d(builder, 0, 0)) polygon_vertices.append(fbh.createPoint2d(builder, 0, 100)) @@ -180,6 +180,32 @@ def polygon2D(cls, builder: Builder) -> Polygon2D.Polygon2D: polygon_vertices.append(fbh.createPoint2d(builder, 100, 0)) return fbh.createPolygon2D(builder, 100, 0, polygon_vertices) + @classmethod + def parameterized_polygon2d( + cls, builder: Builder, quad_extent: float, height: float + ) -> Polygon2D.Polygon2D: + """ + Creates a polygon with a quadratic extent with the + edge points being at (0, 0), (0, quad_extent), + (quad_extent, 0), (quad_extent, quad_extent). + Also the polygon starts at -100 offset height. + + Args: + builder: The flatbuffers builder + quad_extent: The quad_extent on which the other three points + are placed + + Returns: The created Polygon2D type type + """ + polygon_vertices = [] + polygon_vertices.append(fbh.createPoint2d(builder, 0, 0)) + polygon_vertices.append(fbh.createPoint2d(builder, 0, quad_extent)) + polygon_vertices.append( + fbh.createPoint2d(builder, quad_extent, quad_extent) + ) + polygon_vertices.append(fbh.createPoint2d(builder, quad_extent, 0)) + return fbh.createPolygon2D(builder, height, -100, polygon_vertices) + @classmethod def time_interval(cls, builder: Builder) -> TimeInterval.TimeInterval: timeMin = fbh.createTimeStamp(builder, 1610549273, 0) @@ -217,12 +243,8 @@ def ontology_uri(cls, builder: Builder): return None @classmethod - def projectuuid(cls, builder: Builder, channel: Channel) -> List[int]: - return [ - builder.CreateString( - fbh.getProject(builder, channel, "testproject") - ) - ] + def projectuuid(cls, builder: Builder, channel: Channel) -> List[str]: + return [fbh.getProject(builder, channel, "testproject")] @classmethod # returns list of flatbuffered string, those are registered as ints @@ -263,11 +285,7 @@ def intanceuuid( for i in range(0, uuids_by_proj[0].UuidsLength()) ] ) - # serialize the uuid strings - uuids = [ - builder.CreateString(uuids[i]) - for i in range(0, len(uuids), 2) - ] + uuids = [uuids[i] for i in range(0, len(uuids), 2)] return uuids @@ -295,18 +313,9 @@ def datauuid(cls, builder: Builder, channel: Channel) -> List[int]: # print("pcls: " + str(pcl2_uuids)) # fill up return list with every second uuid - ret_lst = [ - builder.CreateString(image_uuids[i]) - for i in range(0, len(image_uuids), 2) - ] - ret_lst += [ - builder.CreateString(point_uuids[i]) - for i in range(0, len(point_uuids), 2) - ] - ret_lst += [ - builder.CreateString(pcl2_uuids[i]) - for i in range(0, len(pcl2_uuids), 2) - ] + ret_lst = [image_uuids[i] for i in range(0, len(image_uuids), 2)] + ret_lst += [point_uuids[i] for i in range(0, len(point_uuids), 2)] + ret_lst += [pcl2_uuids[i] for i in range(0, len(pcl2_uuids), 2)] return ret_lst