Skip to content

Commit

Permalink
Merge pull request #356 from agri-gaia/grpc-decode-arbitrary-pcls
Browse files Browse the repository at this point in the history 
Update FlatBuffers gRPC example to decode queried point clouds
  • Loading branch information
@jarkenau
jarkenau authored Dec 13, 2023
2 parents 3a61507 + 3ae3f0f commit cdd81c4
Show file tree
Hide file tree
Showing 2 changed files with 109 additions and 106 deletions.
191 changes: 85 additions & 106 deletions examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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")
24 changes: 24 additions & 0 deletions examples/python/gRPC/util/fb_helper.py
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import sys

import numpy as np
from seerep.fb import (
Boundingbox,
BoundingBox2DLabeled,
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit cdd81c4

Please sign in to comment.