diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index 140c486e8..28ad59a6b 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -1,11 +1,5 @@ -import struct -import sys - import flatbuffers import numpy as np - -np.set_printoptions(precision=7) - from seerep.fb import PointCloud2 from seerep.fb import point_cloud_service_grpc_fb as pointCloudService from seerep.util.common import get_gRPC_channel @@ -18,111 +12,96 @@ createTimeInterval, createTimeStamp, getProject, + rosToNumpyDtype, ) +PROJECTNAME = "testproject" + +# TODO: move to module in the seerep.fb library +def unpack_point_fields(point_cloud: PointCloud2.PointCloud2) -> dict: + """Extract the point fields from a Flatbuffer pcl message""" + return { + "name": [point_cloud.Fields(i).Name().decode("utf-8") for i in range(point_cloud.FieldsLength())], + "datatype": [point_cloud.Fields(i).Datatype() for i in range(point_cloud.FieldsLength())], + "offset": [point_cloud.Fields(i).Offset() for i in range(point_cloud.FieldsLength())], + "count": [point_cloud.Fields(i).Count() for i in range(point_cloud.FieldsLength())], + } + + +def unpack_header(point_cloud: PointCloud2.PointCloud2) -> dict: + """Extract the header from a Flatbuffer pcl message""" + return { + "uuid_msgs": point_cloud.Header().UuidMsgs().decode("utf-8"), + "uuid_project": point_cloud.Header().UuidProject().decode("utf-8"), + "frame_id": point_cloud.Header().FrameId().decode("utf-8"), + } + + +fb_builder = flatbuffers.Builder(1024) + channel = get_gRPC_channel() +pcl_stub = pointCloudService.PointCloudServiceStub(channel) -stubPointCloud = pointCloudService.PointCloudServiceStub(channel) -builder = flatbuffers.Builder(1024) +if not (projectuuid := getProject(fb_builder, channel, PROJECTNAME)): + print(f"Project: {PROJECTNAME} does not exist, quitting ...") + raise SystemExit -PROJECTNAME = "testproject" -projectuuid = getProject(builder, channel, PROJECTNAME) - -if projectuuid is None: - print(f"Project: {PROJECTNAME} does not exist") - sys.exit() - - -builder = flatbuffers.Builder(1024) - -# Create all necessary objects for the query -l = 10 -polygon_vertices = [] -polygon_vertices.append(createPoint2d(builder, -1.0 * l, -1.0 * l)) -polygon_vertices.append(createPoint2d(builder, -1.0 * l, l)) -polygon_vertices.append(createPoint2d(builder, l, l)) -polygon_vertices.append(createPoint2d(builder, l, -1.0 * l)) -polygon2d = createPolygon2D(builder, 7, -1, polygon_vertices) - -timeMin = createTimeStamp(builder, 1610549273, 0) -timeMax = createTimeStamp(builder, 1938549273, 0) -timeInterval = createTimeInterval(builder, timeMin, timeMax) - -projectUuids = [builder.CreateString(projectuuid)] -# list of categories -category = ["0"] -# list of labels per category -labels = [ - [ - createLabelWithConfidence(builder, "testlabel0"), - createLabelWithConfidence(builder, "testlabelgeneral0"), - ] -] -labelCategory = createLabelWithCategory(builder, category, labels) -dataUuids = [builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248")] -instanceUuids = [builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248")] - -# 4. Create a query with parameters -# all parameters are optional -# with all parameters set (especially with the data and instance uuids set) the result of the query will be empty. Set the query parameters to adequate values or remove them from the query creation +# create a polygon for a spatial query +polygon_vertices = [createPoint2d(fb_builder, x, y) for x in [-10, 10] for y in [-10, 10]] +query_polygon = createPolygon2D(fb_builder, 7, -1, polygon_vertices) + +# create a time interval for a temporal query +time_min, time_max = [createTimeStamp(fb_builder, time) for time in [1610549273, 1938549273]] +time_interval = createTimeInterval(fb_builder, time_min, time_max) + +# create labels for a semantic query +labels = [createLabelWithConfidence(fb_builder, f"testlabel_{i}", 1.0) for i in range(3)] +labels_with_category = createLabelWithCategory(fb_builder, ["default_category"], [labels]) + +# filter for specific data +data_uuids = [fb_builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248")] +instance_uuids = [fb_builder.CreateString("3e12e18d-2d53-40bc-a8af-c5cca3c3b248")] + +project_uuids = [fb_builder.CreateString(projectuuid)] + +# set the query parameters according to your needs query = createQuery( - builder, - # boundingBox=boundingboxStamped, - # timeInterval=timeInterval, - # labels=labelCategory, + fb_builder, + # timeInterval=time_interval, + # labels=labels_with_category, # mustHaveAllLabels=True, - projectUuids=projectUuids, - # instanceUuids=instanceUuids, - # dataUuids=dataUuids, - withoutData=True, - polygon2d=polygon2d, - fullyEncapsulated=True, + projectUuids=project_uuids, + # instanceUuids=instance_uuids, + # dataUuids=data_uuids, + withoutData=False, + # polygon2d=query_polygon, + # fullyEncapsulated=True ) -builder.Finish(query) -buf = builder.Output() - -for responseBuf in stubPointCloud.GetPointCloud2(bytes(buf)): - response = PointCloud2.PointCloud2.GetRootAs(responseBuf) - - print("---Header---") - print(f"Message UUID: {response.Header().UuidMsgs().decode('utf-8')}") - print(f"Project UUID: {response.Header().UuidProject().decode('utf-8')}") - print(f"Frame ID: {response.Header().FrameId().decode('utf-8')}") - - print("---Point Fields---") - for i in range(response.FieldsLength()): - print(f"Field Name: {response.Fields(i).Name().decode('utf-8')}") - print(f"Datatype: {response.Fields(i).Datatype()}") - print(f"Offset: {response.Fields(i).Offset()}") - print(f"Count: {response.Fields(i).Count()}") - - print("---Bounding Box Labels---") - for i in range(response.LabelsBbLength()): - print(f"Label {i}: {response.LabelsBb(i).LabelWithInstance().Label().decode('utf-8')}") - print(f"Instance {i}: {response.LabelsBb(i).LabelWithInstance().InstanceUuid().decode('utf-8')}") - print( - f"Bounding Box Min {i}: " - f"{response.LabelsBb(i).BoundingBoxLabeled(0).BoundingBox().PointMin().X()}," - f"{response.LabelsBb(i).BoundingBoxLabeled(0).BoundingBox().PointMin().Y()}," - f"{response.LabelsBb(i).BoundingBoxLabeled(0).BoundingBox().PointMin().Z()} " - f"(x,y,z)" - ) - print( - f"Bounding Box Max {i}: " - f"{response.LabelsBb(i).BoundingBoxLabeled(0).BoundingBox().PointMax().X()}," - f"{response.LabelsBb(i).BoundingBoxLabeled(0).BoundingBox().PointMax().Y()}," - f"{response.LabelsBb(i).BoundingBoxLabeled(0).BoundingBox().PointMax().Z()} " - f"(x,y,z)" - ) - - print("---General Labels----") - for i in range(response.LabelsGeneralLength()): - print(f"Label {i}: {response.LabelsGeneral(i).Label().decode('utf-8')}") - print(f"Instance {i}: {response.LabelsGeneral(i).InstanceUuid().decode('utf-8')}") - - print("---Data--") - if not response.DataIsNone(): - rawData = response.DataAsNumpy() - data = [struct.unpack('f', rawData[i : i + 4]) for i in range(0, rawData.shape[0], 4)] - reshapedData = np.array(data).reshape(960, 1280, 4) - print(f"Data: {reshapedData}") + +fb_builder.Finish(query) + +for responseBuf in pcl_stub.GetPointCloud2(bytes(fb_builder.Output())): + if response := PointCloud2.PointCloud2.GetRootAs(responseBuf): + print(f"---Header--- \n {unpack_header(response)}") + + point_fields = unpack_point_fields(response) + print(f"---Point Fields--- \n {point_fields}") + + # TODO: add printing of general and bounding box labels + + if not response.DataIsNone(): + dtypes = np.dtype( + { + "names": point_fields["name"], + "formats": [rosToNumpyDtype(datatype) for datatype in point_fields["datatype"]], + "offsets": point_fields["offset"], + "itemsize": response.PointStep(), + } + ) + + decoded_payload = np.frombuffer(response.DataAsNumpy(), dtype=dtypes) + + print(f"---Is dense--- \n {response.IsDense()}") + print(f"---Payload--- \n {decoded_payload}") + else: + print("No response received") diff --git a/examples/python/gRPC/util/fb_helper.py b/examples/python/gRPC/util/fb_helper.py index 61f39d20d..47f732c88 100644 --- a/examples/python/gRPC/util/fb_helper.py +++ b/examples/python/gRPC/util/fb_helper.py @@ -1,5 +1,6 @@ import sys +import numpy as np from seerep.fb import ( Boundingbox, BoundingBox2DLabeled, @@ -41,6 +42,29 @@ from seerep.fb import meta_operations_grpc_fb as metaOperations +# TODO: should be moved into a separate module +def rosToNumpyDtype(ros_dtype: int) -> np.dtype: + """Converts the numeric represenations of dtypes in ROS to numpy dtype objects.""" + if ros_dtype == 1: + return np.dtype(np.int8) + elif ros_dtype == 2: + return np.dtype(np.uint8) + elif ros_dtype == 3: + return np.dtype(np.int16) + elif ros_dtype == 4: + return np.dtype(np.uint16) + elif ros_dtype == 5: + return np.dtype(np.int32) + elif ros_dtype == 6: + return np.dtype(np.uint32) + elif ros_dtype == 7: + return np.dtype(np.float32) + elif ros_dtype == 8: + return np.dtype(np.float64) + else: + raise ValueError("Unknown dtype") + + def getProject(builder, channel, name): '''Retrieve a project by name''' stubMeta = metaOperations.MetaOperationsStub(channel)