diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_addBoundingBox.py b/examples/python/gRPC/pointcloud/gRPC_fb_addBoundingBox.py new file mode 100755 index 000000000..2ff0f7180 --- /dev/null +++ b/examples/python/gRPC/pointcloud/gRPC_fb_addBoundingBox.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +NUM_BB_LABELS = 5 + +import uuid + +import flatbuffers +import numpy as np +from seerep.fb import PointCloud2 +from seerep.fb import point_cloud_service_grpc_fb as pointcloudService +from seerep.util.common import get_gRPC_channel +from seerep.util.fb_helper import ( + createBoundingBoxes, + createBoundingBoxesLabeled, + createBoundingBoxLabeledStamped, + createBoundingBoxLabeledWithCategory, + createHeader, + createLabelsWithInstance, + createPoint, + createQuery, + getProject, +) + +builder = flatbuffers.Builder(1024) +channel = get_gRPC_channel() + +projectuuid = getProject(builder, channel, 'testproject') + +if not projectuuid: + print("Requested project does not exist") + exit() + +stub = pointcloudService.PointCloudServiceStub(channel) + +query = createQuery(builder, projectUuids=[builder.CreateString(projectuuid)], withoutData=True) +builder.Finish(query) +buf = builder.Output() + +msgToSend = [] + +for responseBuf in stub.GetPointCloud2(bytes(buf)): + response = PointCloud2.PointCloud2.GetRootAs(responseBuf) + + header = createHeader( + builder, + projectUuid=response.Header().UuidProject().decode("utf-8"), + msgUuid=response.Header().UuidMsgs().decode("utf-8"), + ) + + # create bounding box labels + x1, y1, z1 = np.random.rand(3) + x2, y2, z2 = np.random.rand(3) + + print("SENDING x1, y1, z1: ", x1, " , ", y1, " , ", z1) + print("SENDING x2, y2, z2: ", x2, " , ", y2, " , ", z2) + + boundingBoxes = createBoundingBoxes( + builder, + [createPoint(builder, x1, y1, z1) for _ in range(NUM_BB_LABELS)], + [createPoint(builder, x2, y2, z2) for _ in range(NUM_BB_LABELS)], + ) + labelWithInstances = createLabelsWithInstance( + builder, + ["BoundingBoxLabel" + str(i) for i in range(NUM_BB_LABELS)], + [1.0 / (i + 0.1) for i in range(NUM_BB_LABELS)], + [str(uuid.uuid4()) for _ in range(NUM_BB_LABELS)], + ) + labelsBb = createBoundingBoxesLabeled(builder, labelWithInstances, boundingBoxes) + + boundingBoxLabeledWithCategory = createBoundingBoxLabeledWithCategory( + builder, builder.CreateString("laterAddedBB"), labelsBb + ) + + labelsBbVector = createBoundingBoxLabeledStamped(builder, header, [boundingBoxLabeledWithCategory]) + builder.Finish(labelsBbVector) + buf = builder.Output() + + msgToSend.append(bytes(buf)) + + +stub.AddBoundingBoxesLabeled(iter(msgToSend)) diff --git a/examples/python/gRPC/util/fb_helper.py b/examples/python/gRPC/util/fb_helper.py index 8178bbd5b..61f39d20d 100644 --- a/examples/python/gRPC/util/fb_helper.py +++ b/examples/python/gRPC/util/fb_helper.py @@ -5,6 +5,7 @@ BoundingBox2DLabeled, BoundingBox2DLabeledWithCategory, BoundingBoxes2DLabeledStamped, + BoundingBoxesLabeledStamped, BoundingBoxLabeled, BoundingBoxLabeledWithCategory, BoundingboxStamped, @@ -302,6 +303,19 @@ def createBoundingBox2dLabeledStamped(builder, header, labelsBb): return BoundingBoxes2DLabeledStamped.End(builder) +def createBoundingBoxLabeledStamped(builder, header, labelsBb): + '''Creates a labeled bounding box in flatbuffers''' + BoundingBoxesLabeledStamped.StartLabelsBbVector(builder, len(labelsBb)) + for labelBb in reversed(labelsBb): + builder.PrependUOffsetTRelative(labelBb) + labelsBbVector = builder.EndVector() + + BoundingBoxesLabeledStamped.Start(builder) + BoundingBoxesLabeledStamped.AddHeader(builder, header) + BoundingBoxesLabeledStamped.AddLabelsBb(builder, labelsBbVector) + return BoundingBoxesLabeledStamped.End(builder) + + def createBoundingBox2DLabeledWithCategory(builder, category, bb2dLabeled): BoundingBox2DLabeledWithCategory.StartBoundingBox2dLabeledVector(builder, len(bb2dLabeled)) for labelBb in reversed(bb2dLabeled): diff --git a/seerep_com/fbs/point_cloud_service.fbs b/seerep_com/fbs/point_cloud_service.fbs index da81bcf43..6100d8d7a 100644 --- a/seerep_com/fbs/point_cloud_service.fbs +++ b/seerep_com/fbs/point_cloud_service.fbs @@ -1,6 +1,7 @@ include "point_cloud_2.fbs"; include "query.fbs"; +include "boundingboxes_labeled_stamped.fbs"; include "server_response.fbs"; @@ -9,4 +10,5 @@ namespace seerep.fb; rpc_service PointCloudService { GetPointCloud2(seerep.fb.Query):seerep.fb.PointCloud2 (streaming: "server"); TransferPointCloud2(seerep.fb.PointCloud2):seerep.fb.ServerResponse (streaming: "client"); + AddBoundingBoxesLabeled(seerep.fb.BoundingBoxesLabeledStamped):seerep.fb.ServerResponse (streaming: "client"); } diff --git a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_pointcloud.h b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_pointcloud.h index c4962871c..07a9bdf7e 100644 --- a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_pointcloud.h +++ b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_pointcloud.h @@ -42,7 +42,15 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral */ void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2, std::vector& boundingBox); - + /** + * @brief Write a BoundingBoxes flatbuffers message to hdf5 + * + * @param id the uuid of the image data group + * @param bbLabeledWithCategory the flatbuffers BoundingBoxes with category message + */ + void writePointCloudBoundingBoxLabeled( + const std::string& id, + const flatbuffers::Vector>* bbLabeledWithCategory); /** * @brief Method for reading a flatbuffers PointCloud2 message from hdf5 * diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp index a5017339d..3bdbcf9a4 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp @@ -79,13 +79,26 @@ void Hdf5FbGeneral::writeBoundingBoxLabeled(const std::string& datatypeGroup, co { labels.push_back(label->labelWithInstance()->label()->label()->str()); labelConfidence.push_back(label->labelWithInstance()->label()->confidence()); - std::vector boxWithRotation{ - label->bounding_box()->center_point()->x(), label->bounding_box()->center_point()->y(), - label->bounding_box()->center_point()->z(), label->bounding_box()->spatial_extent()->x(), - label->bounding_box()->spatial_extent()->y(), label->bounding_box()->spatial_extent()->z(), - label->bounding_box()->rotation()->x(), label->bounding_box()->rotation()->y(), - label->bounding_box()->rotation()->z(), label->bounding_box()->rotation()->w() - }; + + double rotX = 0.0, rotY = 0.0, rotZ = 0.0, rotW = 1.0; + if (label->bounding_box()->rotation() != NULL) + { + rotX = label->bounding_box()->rotation()->x(); + rotY = label->bounding_box()->rotation()->y(); + rotZ = label->bounding_box()->rotation()->z(); + rotW = label->bounding_box()->rotation()->w(); + } + + std::vector boxWithRotation{ label->bounding_box()->center_point()->x(), + label->bounding_box()->center_point()->y(), + label->bounding_box()->center_point()->z(), + label->bounding_box()->spatial_extent()->x(), + label->bounding_box()->spatial_extent()->y(), + label->bounding_box()->spatial_extent()->z(), + rotX, + rotY, + rotZ, + rotW }; boundingBoxesWithRotation.push_back(boxWithRotation); instances.push_back(label->labelWithInstance()->instanceUuid()->str()); diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp index a6fd5c275..c8b103dad 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp @@ -208,6 +208,15 @@ void Hdf5FbPointCloud::writeGeneralAttributes(std::shared_ptr& writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, cloud.is_dense()); } +void Hdf5FbPointCloud::writePointCloudBoundingBoxLabeled( + const std::string& id, + const flatbuffers::Vector>* bbLabeledWithCategory) +{ + const std::scoped_lock lock(*m_write_mtx); + + writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, bbLabeledWithCategory); +} + // TODO partial point cloud read std::optional> Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 3ef4084ef..1e26c84a4 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -80,6 +80,7 @@ set(MY_FBS_FILES fbs/boundingbox2d_labeled_with_category.fbs fbs/boundingbox2d_stamped.fbs fbs/boundingbox2d.fbs + fbs/boundingboxes_labeled_stamped.fbs fbs/boundingboxes2d_labeled_stamped.fbs fbs/camera_intrinsics.fbs fbs/camera_intrinsics_query.fbs diff --git a/seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs b/seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs new file mode 100644 index 000000000..98599515e --- /dev/null +++ b/seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs @@ -0,0 +1,10 @@ + +include "header.fbs"; +include "boundingbox_labeled_with_category.fbs"; + +namespace seerep.fb; + +table BoundingBoxesLabeledStamped { + header:seerep.fb.Header; + labels_bb:[seerep.fb.BoundingBoxLabeledWithCategory]; +} diff --git a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_pointcloud.h b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_pointcloud.h index f25d26ade..c10d15f16 100644 --- a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_pointcloud.h +++ b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_pointcloud.h @@ -62,9 +62,14 @@ class CoreFbPointCloud * @brief Add a point cloud to the indices and write the data to hdf5 * * @param pc the point cloud message to index and save - * @return boost::uuids::uuid the uuid of the stored image + * @return boost::uuids::uuid the uuid of the stored pointcloud */ boost::uuids::uuid addData(const seerep::fb::PointCloud2& pc); + /** + * @brief Adds bounding box based labels to an existing pointcloud + * @param bbslabeled the flatbuffer message containing bounding box based labels + */ + void addBoundingBoxesLabeled(const seerep::fb::BoundingBoxesLabeledStamped& boundingBoxeslabeled); private: /** @brief a shared pointer to the general core */ diff --git a/seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp b/seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp index 8bf89bda5..99719eef9 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp @@ -56,4 +56,48 @@ boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc) return dataForIndices.header.uuidData; } +void CoreFbPointCloud::addBoundingBoxesLabeled(const seerep::fb::BoundingBoxesLabeledStamped& boundingBoxeslabeled) +{ + boost::uuids::string_generator gen; + boost::uuids::uuid uuidMsg = gen(boundingBoxeslabeled.header()->uuid_msgs()->str()); + boost::uuids::uuid uuidProject = gen(boundingBoxeslabeled.header()->uuid_project()->str()); + + auto hdf5io = CoreFbGeneral::getHdf5(uuidProject, m_seerepCore, m_hdf5IoMap); + + hdf5io->writePointCloudBoundingBoxLabeled(boost::lexical_cast(uuidMsg), boundingBoxeslabeled.labels_bb()); + + std::unordered_map> labelWithInstancePerCategory; + for (auto bbWithCategory : *boundingBoxeslabeled.labels_bb()) + { + if (bbWithCategory->boundingBoxLabeled()) + { + std::vector labelsWithInstances; + for (auto bb : *bbWithCategory->boundingBoxLabeled()) + { + seerep_core_msgs::LabelWithInstance labelWithInstance; + labelWithInstance.label = bb->labelWithInstance()->label()->label()->str(); + labelWithInstance.labelConfidence = bb->labelWithInstance()->label()->confidence(); + + try + { + labelWithInstance.uuidInstance = gen(bb->labelWithInstance()->instanceUuid()->str()); + } + catch (std::runtime_error const& e) + { + labelWithInstance.uuidInstance = boost::uuids::nil_uuid(); + } + labelsWithInstances.push_back(labelWithInstance); + } + labelWithInstancePerCategory.emplace(bbWithCategory->category()->c_str(), labelsWithInstances); + } + // this only adds labels to the pointcloud in the core + // if there are already bounding box labels for this pointcloud + // those labels must be removed separately. The hdfio currently overrides + // existing labels. The data is only correct if labels are added and there + // weren't any bounding box labels before + + m_seerepCore->addLabels(seerep_core_msgs::Datatype::PointCloud, labelWithInstancePerCategory, uuidMsg, uuidProject); + } +} + } /* namespace seerep_core_fb */ diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_point_cloud_service.h b/seerep_srv/seerep_server/include/seerep_server/fb_point_cloud_service.h index 245c80e19..ee757fcfe 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_point_cloud_service.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_point_cloud_service.h @@ -50,6 +50,18 @@ class FbPointCloudService final : public seerep::fb::PointCloudService::Service grpc::Status TransferPointCloud2(grpc::ServerContext* context, grpc::ServerReader>* reader, flatbuffers::grpc::Message* response) override; + /** + * @brief Adds a stream of labeled bounding boxes to already stored point clouds + * + * @param context custom inital and trailing metadata (currently not used) + * @param reader incoming message stream of bounding boxes from the client + * @param response gRPC message to describe the transmission state of the bounding boxes + * @return grpc::Status status of the request. Did it work? + */ + grpc::Status AddBoundingBoxesLabeled( + grpc::ServerContext* context, + grpc::ServerReader>* reader, + flatbuffers::grpc::Message* response) override; private: /** @brief a shared pointer to the general core */ diff --git a/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp b/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp index 9b0fcdc66..f6fa7593b 100644 --- a/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp +++ b/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp @@ -135,4 +135,69 @@ grpc::Status FbPointCloudService::TransferPointCloud2( return grpc::Status::OK; } +grpc::Status FbPointCloudService::AddBoundingBoxesLabeled( + grpc::ServerContext* context, + grpc::ServerReader>* reader, + flatbuffers::grpc::Message* response) +{ + (void)context; // ignore that variable without causing warnings + std::string answer = "everything stored!"; + + flatbuffers::grpc::Message bbsMsg; + while (reader->Read(&bbsMsg)) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "received BoundingBoxesLabeledStamped... "; + auto bbslabeled = bbsMsg.GetRoot(); + + std::string uuidProject = bbslabeled->header()->uuid_project()->str(); + if (uuidProject.empty()) + { + answer = "a msg had no project uuid!"; + } + else + { + if (!bbslabeled->labels_bb()) + { + answer = "a msg had no bounding boxes!"; + } + else + { + try + { + pointCloudFb->addBoundingBoxesLabeled(*bbslabeled); + } + 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(); + + seerep_server_util::createResponseFb(std::string(e.what()), seerep::fb::TRANSMISSION_STATE_FAILURE, response); + + 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(); + seerep_server_util::createResponseFb(std::string(e.what()), seerep::fb::TRANSMISSION_STATE_FAILURE, response); + 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; + seerep_server_util::createResponseFb(msg, seerep::fb::TRANSMISSION_STATE_FAILURE, response); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, msg); + } + } + } + } + seerep_server_util::createResponseFb(answer, seerep::fb::TRANSMISSION_STATE_SUCCESS, response); + + return grpc::Status::OK; +} + } /* namespace seerep_server */