From bce2a1a08b77805afa95de5583733d1c468e68d5 Mon Sep 17 00:00:00 2001 From: Mark Niemeyer Date: Thu, 4 Jan 2024 20:58:44 +0100 Subject: [PATCH] WIP: use datumaro for labels and make labels agnostic of annotation type --- examples/cpp/gRPC/CATKIN_IGNORE | 0 examples/cpp/gRPC/src/queryImages.cpp | 2 +- .../seerep_ros_communication/CATKIN_IGNORE | 0 seerep_com/CMakeLists.txt | 2 - seerep_com/fbs/image_service.fbs | 4 +- seerep_com/fbs/meta_operations.fbs | 7 +- seerep_com/fbs/point_cloud_service.fbs | 4 +- seerep_com/fbs/tf_service.fbs | 4 +- seerep_com/fbs/transfer_sensor_msgs.fbs | 18 - seerep_com/protos/meta_operations.proto | 7 +- seerep_com/protos/tf_service.proto | 5 +- seerep_com/protos/transfer_sensor_msgs.proto | 20 - .../seerep_hdf5_core/hdf5_core_general.h | 71 ++-- .../seerep_hdf5_core/hdf5_core_image.h | 8 +- .../impl/hdf5_core_general.hpp | 10 + .../src/hdf5_core_general.cpp | 215 +++------- .../seerep_hdf5_core/src/hdf5_core_image.cpp | 14 +- .../seerep_hdf5_core/src/hdf5_core_point.cpp | 2 +- .../src/hdf5_core_point_cloud.cpp | 6 +- .../include/seerep_hdf5_fb/hdf5_fb_general.h | 87 +--- .../include/seerep_hdf5_fb/hdf5_fb_image.h | 12 - .../seerep_hdf5_fb/hdf5_fb_pointcloud.h | 9 - .../seerep_hdf5_fb/src/hdf5_fb_general.cpp | 391 +++--------------- .../seerep_hdf5_fb/src/hdf5_fb_image.cpp | 20 +- .../seerep_hdf5_fb/src/hdf5_fb_point.cpp | 6 +- .../seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp | 22 +- .../test/fb_write_load_test.cpp | 136 ++---- .../include/seerep_hdf5_pb/hdf5_pb_general.h | 29 +- .../seerep_hdf5_pb/src/hdf5_pb_general.cpp | 227 ++-------- .../seerep_hdf5_pb/src/hdf5_pb_image.cpp | 15 +- .../seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp | 3 +- .../test/pb_write_load_test.cpp | 120 ++---- seerep_hdf5/seerep_hdf5_py/CATKIN_IGNORE | 0 seerep_msgs/CMakeLists.txt | 55 +-- seerep_msgs/core/dataset_indexable.h | 4 +- seerep_msgs/core/label.h | 19 + seerep_msgs/core/label_category.h | 20 + seerep_msgs/core/label_with_instance.h | 18 - .../core/labels_with_instance_with_category.h | 20 - seerep_msgs/fbs/boundingbox2d.fbs | 10 - seerep_msgs/fbs/boundingbox2d_labeled.fbs | 10 - .../boundingbox2d_labeled_with_category.fbs | 9 - seerep_msgs/fbs/boundingbox2d_stamped.fbs | 10 - seerep_msgs/fbs/boundingbox_labeled.fbs | 10 - .../fbs/boundingbox_labeled_with_category.fbs | 10 - seerep_msgs/fbs/boundingbox_stamped.fbs | 10 - .../fbs/boundingboxes2d_labeled_stamped.fbs | 10 - .../fbs/boundingboxes_labeled_stamped.fbs | 10 - seerep_msgs/fbs/categories.fbs | 6 - seerep_msgs/fbs/frame_infos.fbs | 6 - seerep_msgs/fbs/image.fbs | 6 +- seerep_msgs/fbs/label.fbs | 4 +- ...s_with_category.fbs => label_category.fbs} | 3 +- seerep_msgs/fbs/label_with_instance.fbs | 9 - seerep_msgs/fbs/labels.fbs | 6 - .../labels_with_instance_with_category.fbs | 9 - seerep_msgs/fbs/point_cloud_2.fbs | 6 +- seerep_msgs/fbs/point_stamped.fbs | 4 +- seerep_msgs/fbs/polygon.fbs | 8 - seerep_msgs/fbs/polygon_stamped.fbs | 10 - seerep_msgs/fbs/pose.fbs | 10 - seerep_msgs/fbs/pose_stamped.fbs | 10 - seerep_msgs/fbs/quaternion_stamped.fbs | 10 - seerep_msgs/fbs/query.fbs | 4 +- seerep_msgs/fbs/string_vector.fbs | 6 + seerep_msgs/fbs/twist.fbs | 9 - seerep_msgs/fbs/twist_stamped.fbs | 10 - seerep_msgs/fbs/twist_with_covariance.fbs | 9 - .../fbs/twist_with_covariance_stamped.fbs | 10 - seerep_msgs/fbs/vector3_stamped.fbs | 10 - seerep_msgs/protos/boundingbox2d.proto | 12 - .../protos/boundingbox2d_labeled.proto | 12 - .../boundingbox2d_labeled_with_category.proto | 11 - .../protos/boundingbox2d_stamped.proto | 12 - seerep_msgs/protos/boundingbox_labeled.proto | 12 - .../boundingbox_labeled_with_category.proto | 11 - seerep_msgs/protos/boundingbox_stamped.proto | 12 - seerep_msgs/protos/categories.proto | 8 - seerep_msgs/protos/frame_infos.proto | 8 - seerep_msgs/protos/image.proto | 10 +- seerep_msgs/protos/label.proto | 4 +- ...th_category.proto => label_category.proto} | 3 +- seerep_msgs/protos/label_with_instance.proto | 11 - seerep_msgs/protos/labels.proto | 8 - .../labels_with_instance_with_category.proto | 11 - seerep_msgs/protos/point_cloud_2.proto | 8 +- seerep_msgs/protos/point_stamped.proto | 4 +- seerep_msgs/protos/polygon.proto | 10 - seerep_msgs/protos/polygon_stamped.proto | 12 - seerep_msgs/protos/pose.proto | 12 - seerep_msgs/protos/pose_stamped.proto | 12 - seerep_msgs/protos/quaternion_stamped.proto | 12 - seerep_msgs/protos/query.proto | 4 +- seerep_msgs/protos/string_vector.proto | 8 + seerep_msgs/protos/twist.proto | 11 - seerep_msgs/protos/twist_stamped.proto | 12 - .../protos/twist_with_covariance.proto | 20 - .../twist_with_covariance_stamped.proto | 12 - seerep_msgs/protos/vector3_stamped.proto | 12 - .../seerep_ros_conversions_fb/conversions.h | 208 +++------- .../seerep_ros_conversions_fb/conversions.cpp | 318 ++++++-------- .../seerep_ros_conversions_pb/conversions.h | 53 +-- .../seerep_ros_conversions_pb/conversions.cpp | 57 --- .../seerep_core/include/seerep_core/core.h | 5 +- .../include/seerep_core/core_dataset.h | 10 +- .../include/seerep_core/core_instance.h | 1 - .../include/seerep_core/core_instances.h | 11 +- .../include/seerep_core/core_project.h | 5 +- seerep_srv/seerep_core/src/core.cpp | 5 +- seerep_srv/seerep_core/src/core_dataset.cpp | 18 +- seerep_srv/seerep_core/src/core_instances.cpp | 26 +- seerep_srv/seerep_core/src/core_project.cpp | 5 +- .../seerep_core_fb/core_fb_conversion.h | 22 +- .../include/seerep_core_fb/core_fb_image.h | 12 +- .../seerep_core_fb/core_fb_pointcloud.h | 12 +- .../seerep_core_fb/src/core_fb_conversion.cpp | 129 ++---- .../seerep_core_fb/src/core_fb_image.cpp | 86 ++-- .../seerep_core_fb/src/core_fb_pointcloud.cpp | 86 ++-- .../seerep_core_pb/core_pb_conversion.h | 5 +- .../seerep_core_pb/src/core_pb_conversion.cpp | 68 +-- .../seerep_core_pb/src/core_pb_pointcloud.cpp | 61 +-- .../include/seerep_server/fb_image_service.h | 9 +- .../seerep_server/fb_meta_operations.h | 4 +- .../seerep_server/fb_point_cloud_service.h | 9 +- .../include/seerep_server/fb_tf_service.h | 2 +- .../seerep_server/pb_meta_operations.h | 4 +- .../seerep_server/pb_receive_sensor_msgs.h | 47 --- .../include/seerep_server/pb_tf_service.h | 2 +- .../seerep_server/src/fb_image_service.cpp | 128 +++--- .../seerep_server/src/fb_meta_operations.cpp | 16 +- .../src/fb_point_cloud_service.cpp | 128 +++--- .../seerep_server/src/fb_tf_service.cpp | 8 +- .../seerep_server/src/pb_meta_operations.cpp | 8 +- .../src/pb_receive_sensor_msgs.cpp | 59 --- .../seerep_server/src/pb_tf_service.cpp | 4 +- 135 files changed, 903 insertions(+), 2888 deletions(-) create mode 100644 examples/cpp/gRPC/CATKIN_IGNORE create mode 100644 examples/cpp/seerep_ros_communication/CATKIN_IGNORE delete mode 100644 seerep_com/fbs/transfer_sensor_msgs.fbs delete mode 100644 seerep_com/protos/transfer_sensor_msgs.proto create mode 100644 seerep_hdf5/seerep_hdf5_py/CATKIN_IGNORE create mode 100644 seerep_msgs/core/label.h create mode 100644 seerep_msgs/core/label_category.h delete mode 100644 seerep_msgs/core/label_with_instance.h delete mode 100644 seerep_msgs/core/labels_with_instance_with_category.h delete mode 100644 seerep_msgs/fbs/boundingbox2d.fbs delete mode 100644 seerep_msgs/fbs/boundingbox2d_labeled.fbs delete mode 100644 seerep_msgs/fbs/boundingbox2d_labeled_with_category.fbs delete mode 100644 seerep_msgs/fbs/boundingbox2d_stamped.fbs delete mode 100644 seerep_msgs/fbs/boundingbox_labeled.fbs delete mode 100644 seerep_msgs/fbs/boundingbox_labeled_with_category.fbs delete mode 100644 seerep_msgs/fbs/boundingbox_stamped.fbs delete mode 100644 seerep_msgs/fbs/boundingboxes2d_labeled_stamped.fbs delete mode 100644 seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs delete mode 100644 seerep_msgs/fbs/categories.fbs delete mode 100644 seerep_msgs/fbs/frame_infos.fbs rename seerep_msgs/fbs/{labels_with_category.fbs => label_category.fbs} (65%) delete mode 100644 seerep_msgs/fbs/label_with_instance.fbs delete mode 100644 seerep_msgs/fbs/labels.fbs delete mode 100644 seerep_msgs/fbs/labels_with_instance_with_category.fbs delete mode 100644 seerep_msgs/fbs/polygon.fbs delete mode 100644 seerep_msgs/fbs/polygon_stamped.fbs delete mode 100644 seerep_msgs/fbs/pose.fbs delete mode 100644 seerep_msgs/fbs/pose_stamped.fbs delete mode 100644 seerep_msgs/fbs/quaternion_stamped.fbs create mode 100644 seerep_msgs/fbs/string_vector.fbs delete mode 100644 seerep_msgs/fbs/twist.fbs delete mode 100644 seerep_msgs/fbs/twist_stamped.fbs delete mode 100644 seerep_msgs/fbs/twist_with_covariance.fbs delete mode 100644 seerep_msgs/fbs/twist_with_covariance_stamped.fbs delete mode 100644 seerep_msgs/fbs/vector3_stamped.fbs delete mode 100644 seerep_msgs/protos/boundingbox2d.proto delete mode 100644 seerep_msgs/protos/boundingbox2d_labeled.proto delete mode 100644 seerep_msgs/protos/boundingbox2d_labeled_with_category.proto delete mode 100644 seerep_msgs/protos/boundingbox2d_stamped.proto delete mode 100644 seerep_msgs/protos/boundingbox_labeled.proto delete mode 100644 seerep_msgs/protos/boundingbox_labeled_with_category.proto delete mode 100644 seerep_msgs/protos/boundingbox_stamped.proto delete mode 100644 seerep_msgs/protos/categories.proto delete mode 100644 seerep_msgs/protos/frame_infos.proto rename seerep_msgs/protos/{labels_with_category.proto => label_category.proto} (70%) delete mode 100644 seerep_msgs/protos/label_with_instance.proto delete mode 100644 seerep_msgs/protos/labels.proto delete mode 100644 seerep_msgs/protos/labels_with_instance_with_category.proto delete mode 100644 seerep_msgs/protos/polygon.proto delete mode 100644 seerep_msgs/protos/polygon_stamped.proto delete mode 100644 seerep_msgs/protos/pose.proto delete mode 100644 seerep_msgs/protos/pose_stamped.proto delete mode 100644 seerep_msgs/protos/quaternion_stamped.proto create mode 100644 seerep_msgs/protos/string_vector.proto delete mode 100644 seerep_msgs/protos/twist.proto delete mode 100644 seerep_msgs/protos/twist_stamped.proto delete mode 100644 seerep_msgs/protos/twist_with_covariance.proto delete mode 100644 seerep_msgs/protos/twist_with_covariance_stamped.proto delete mode 100644 seerep_msgs/protos/vector3_stamped.proto delete mode 100644 seerep_srv/seerep_server/include/seerep_server/pb_receive_sensor_msgs.h delete mode 100644 seerep_srv/seerep_server/src/pb_receive_sensor_msgs.cpp diff --git a/examples/cpp/gRPC/CATKIN_IGNORE b/examples/cpp/gRPC/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/examples/cpp/gRPC/src/queryImages.cpp b/examples/cpp/gRPC/src/queryImages.cpp index 2cc63cedd..9f47dbf2a 100644 --- a/examples/cpp/gRPC/src/queryImages.cpp +++ b/examples/cpp/gRPC/src/queryImages.cpp @@ -81,7 +81,7 @@ flatbuffers::Offset createPoint(flatbuffers::grpc::MessageBui return pointBuilder.Finish(); } -flatbuffers::Offset>> +flatbuffers::Offset>> createLabelWithInstance(flatbuffers::grpc::MessageBuilder& mb, const seerep::fb::BoundingBox2DLabeled* bb, const flatbuffers::String* category) { diff --git a/examples/cpp/seerep_ros_communication/CATKIN_IGNORE b/examples/cpp/seerep_ros_communication/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/seerep_com/CMakeLists.txt b/seerep_com/CMakeLists.txt index 0117c26fb..55ec8bb58 100644 --- a/seerep_com/CMakeLists.txt +++ b/seerep_com/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(Flatbuffers REQUIRED) list(APPEND PROTOBUF_IMPORT_DIRS ${SeerepMsgs_PROTOBUF_IMPORT_DIRS}) set(MY_PROTO_FILES - protos/transfer_sensor_msgs.proto protos/meta_operations.proto protos/tf_service.proto protos/image_service.proto @@ -32,7 +31,6 @@ set(MY_FBS_FILES fbs/meta_operations.fbs fbs/point_service.fbs fbs/tf_service.fbs - fbs/transfer_sensor_msgs.fbs fbs/point_cloud_service.fbs fbs/camera_intrinsics_service.fbs ) diff --git a/seerep_com/fbs/image_service.fbs b/seerep_com/fbs/image_service.fbs index dcf96c25d..f42e3fbb9 100644 --- a/seerep_com/fbs/image_service.fbs +++ b/seerep_com/fbs/image_service.fbs @@ -1,7 +1,7 @@ include "image.fbs"; include "query.fbs"; -include "boundingboxes2d_labeled_stamped.fbs"; +include "label_category.fbs"; include "server_response.fbs"; @@ -10,5 +10,5 @@ namespace seerep.fb; rpc_service ImageService { GetImage(seerep.fb.Query):seerep.fb.Image (streaming: "server"); TransferImage(seerep.fb.Image):seerep.fb.ServerResponse (streaming: "client"); - AddBoundingBoxes2dLabeled(seerep.fb.BoundingBoxes2DLabeledStamped):seerep.fb.ServerResponse (streaming: "client"); + AddBoundingBoxes2dLabeled(seerep.fb.LabelCategory):seerep.fb.ServerResponse (streaming: "client"); } diff --git a/seerep_com/fbs/meta_operations.fbs b/seerep_com/fbs/meta_operations.fbs index 003764260..15f0bc483 100644 --- a/seerep_com/fbs/meta_operations.fbs +++ b/seerep_com/fbs/meta_operations.fbs @@ -7,8 +7,7 @@ include "boundingbox.fbs"; include "time_interval.fbs"; include "uuid_datatype_pair.fbs"; include "uuid_datatype_with_category.fbs"; -include "labels.fbs"; -include "categories.fbs"; +include "string_vector.fbs"; namespace seerep.fb; @@ -19,6 +18,6 @@ rpc_service MetaOperations { DeleteProject(seerep.fb.ProjectInfo):seerep.fb.Empty; GetOverallTimeInterval(seerep.fb.UuidDatatypePair):seerep.fb.TimeInterval; GetOverallBoundingBox(seerep.fb.UuidDatatypePair):seerep.fb.Boundingbox; - GetAllCategories(seerep.fb.UuidDatatypePair):seerep.fb.Categories; - GetAllLabels(seerep.fb.UuidDatatypeWithCategory):seerep.fb.Labels; + GetAllCategories(seerep.fb.UuidDatatypePair):seerep.fb.StringVector; + GetAllLabels(seerep.fb.UuidDatatypeWithCategory):seerep.fb.StringVector; } diff --git a/seerep_com/fbs/point_cloud_service.fbs b/seerep_com/fbs/point_cloud_service.fbs index 6100d8d7a..1987581be 100644 --- a/seerep_com/fbs/point_cloud_service.fbs +++ b/seerep_com/fbs/point_cloud_service.fbs @@ -1,7 +1,7 @@ include "point_cloud_2.fbs"; include "query.fbs"; -include "boundingboxes_labeled_stamped.fbs"; +include "label_category.fbs"; include "server_response.fbs"; @@ -10,5 +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"); + AddBoundingBoxesLabeled(seerep.fb.LabelCategory):seerep.fb.ServerResponse (streaming: "client"); } diff --git a/seerep_com/fbs/tf_service.fbs b/seerep_com/fbs/tf_service.fbs index 82fe14c4e..6e03da1e1 100644 --- a/seerep_com/fbs/tf_service.fbs +++ b/seerep_com/fbs/tf_service.fbs @@ -3,13 +3,13 @@ include "transform_stamped.fbs"; include "transform_stamped_query.fbs"; include "server_response.fbs"; -include "frame_infos.fbs"; +include "string_vector.fbs"; include "frame_query.fbs"; namespace seerep.fb; rpc_service TfService { TransferTransformStamped(seerep.fb.TransformStamped):seerep.fb.ServerResponse (streaming: "client"); - GetFrames(seerep.fb.FrameQuery):seerep.fb.FrameInfos; + GetFrames(seerep.fb.FrameQuery):seerep.fb.StringVector; GetTransformStamped(seerep.fb.TransformStampedQuery):seerep.fb.TransformStamped; } diff --git a/seerep_com/fbs/transfer_sensor_msgs.fbs b/seerep_com/fbs/transfer_sensor_msgs.fbs deleted file mode 100644 index 0a3e26b2b..000000000 --- a/seerep_com/fbs/transfer_sensor_msgs.fbs +++ /dev/null @@ -1,18 +0,0 @@ - -include "header.fbs"; -include "point.fbs"; -include "pose_stamped.fbs"; -include "pose.fbs"; -include "quaternion.fbs"; - -include "server_response.fbs"; - -namespace seerep.fb; - -rpc_service TransferSensorMsgs { - TransferHeader(seerep.fb.Header):seerep.fb.ServerResponse; - TransferPoint(seerep.fb.Point):seerep.fb.ServerResponse; - TransferQuaternion(seerep.fb.Quaternion):seerep.fb.ServerResponse; - TransferPose(seerep.fb.Pose):seerep.fb.ServerResponse; - TransferPoseStamped(seerep.fb.PoseStamped):seerep.fb.ServerResponse; -} diff --git a/seerep_com/protos/meta_operations.proto b/seerep_com/protos/meta_operations.proto index 14946646c..d89a5f423 100644 --- a/seerep_com/protos/meta_operations.proto +++ b/seerep_com/protos/meta_operations.proto @@ -10,9 +10,8 @@ import "projectCreation.proto"; import "uuid_datatype_pair.proto"; import "time_interval.proto"; import "boundingbox.proto"; -import "labels.proto"; -import "categories.proto"; import "uuid_datatype_with_category.proto"; +import "string_vector.proto"; service MetaOperations { @@ -20,6 +19,6 @@ service MetaOperations rpc GetProjects(google.protobuf.Empty) returns (ProjectInfos); rpc GetOverallTimeInterval(UuidDatatypePair) returns (TimeInterval); rpc GetOverallBoundingBox(UuidDatatypePair) returns (Boundingbox); - rpc GetAllCategories(UuidDatatypePair) returns (Categories); - rpc GetAllLabels(UuidDatatypeWithCategory) returns (Labels); + rpc GetAllCategories(UuidDatatypePair) returns (StringVector); + rpc GetAllLabels(UuidDatatypeWithCategory) returns (StringVector); } diff --git a/seerep_com/protos/tf_service.proto b/seerep_com/protos/tf_service.proto index ec1e2eafd..e802062a0 100644 --- a/seerep_com/protos/tf_service.proto +++ b/seerep_com/protos/tf_service.proto @@ -4,15 +4,14 @@ package seerep.pb; import "transform_stamped.proto"; import "transform_stamped_query.proto"; -// import "query.proto"; import "server_response.proto"; -import "frame_infos.proto"; import "frame_query.proto"; +import "string_vector.proto"; service TfService { rpc TransferTransformStamped(TransformStamped) returns (ServerResponse); - rpc GetFrames(FrameQuery) returns (FrameInfos); + rpc GetFrames(FrameQuery) returns (StringVector); rpc GetTransformStamped(TransformStampedQuery) returns (TransformStamped); } diff --git a/seerep_com/protos/transfer_sensor_msgs.proto b/seerep_com/protos/transfer_sensor_msgs.proto deleted file mode 100644 index 97d22012d..000000000 --- a/seerep_com/protos/transfer_sensor_msgs.proto +++ /dev/null @@ -1,20 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "point.proto"; -import "pose_stamped.proto"; -import "pose.proto"; -import "quaternion.proto"; - -import "server_response.proto"; - -service TransferSensorMsgs -{ - rpc TransferHeader(Header) returns (ServerResponse); - rpc TransferPoint(Point) returns (ServerResponse); - rpc TransferQuaternion(Quaternion) returns (ServerResponse); - rpc TransferPose(Pose) returns (ServerResponse); - rpc TransferPoseStamped(PoseStamped) returns (ServerResponse); -} diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_general.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_general.h index 1424f5b59..f44a2828f 100644 --- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_general.h +++ b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_general.h @@ -8,7 +8,8 @@ #include #include #include -#include +#include +#include // std #include @@ -147,23 +148,23 @@ class Hdf5CoreGeneral template void readHeader(const std::string& id, HighFive::AnnotateTraits& object, seerep_core_msgs::Header& header); - // BoundingBoxes - /** - * @brief read the labels and instances of a dataset of all categories and add them to the category-labels/instances map - * - * @param [in] datatypeGroup the data type - * @param [in] uuid the uuid of the dataset - * @param [in,out] labelsWithInstancesWithCategory the map from category to the instances of the category - */ - void readBoundingBoxLabeledAndAddToLabelsWithInstancesWithCategory( - const std::string& datatypeGroup, const std::string& uuid, - std::unordered_map>& labelsWithInstancesWithCategory); - void readBoundingBoxLabeled(const std::string& datatypeGroup, const std::string& uuid, - std::vector& labelCategories, - std::vector>& labelsPerCategory, - std::vector>& labelConfidencesPerCategory, - std::vector>>& boundingBoxesPerCategory, - std::vector>& instancesPerCategory, bool loadBoxes = true); + // // BoundingBoxes + // /** + // * @brief read the labels and instances of a dataset of all categories and add them to the category-labels/instances map + // * + // * @param [in] datatypeGroup the data type + // * @param [in] uuid the uuid of the dataset + // * @param [in,out] labelsWithInstancesWithCategory the map from category to the instances of the category + // */ + // void readBoundingBoxLabeledAndAddToLabelsWithInstancesWithCategory( + // const std::string& datatypeGroup, const std::string& uuid, + // std::unordered_map>& labelsWithInstancesWithCategory); + // void readBoundingBoxLabeled(const std::string& datatypeGroup, const std::string& uuid, + // std::vector& labelCategories, + // std::vector>& labelsPerCategory, + // std::vector>& labelConfidencesPerCategory, + // std::vector>>& boundingBoxesPerCategory, + // std::vector>& instancesPerCategory, bool loadBoxes = true); // Labels General /** @@ -174,15 +175,14 @@ class Hdf5CoreGeneral * @param [in] uuid the uuid of the dataset * @param [in,out] labelsWithInstancesWithCategory the map from category to the instances of the category */ - void readLabelsGeneralAndAddToLabelsWithInstancesWithCategory( + void readLabelsAndAddToLabelsPerCategory( const std::string& datatypeGroup, const std::string& uuid, - std::unordered_map>& labelsWithInstancesWithCategory); - void readLabelsGeneral( - const std::string& datatypeGroup, const std::string& uuid, std::vector& labelCategories, - std::vector>& labelsWithInstancesGeneralPerCategory); - void writeLabelsGeneral( - const std::string& datatypeGroup, const std::string& uuid, - const std::vector& labelsWithInstanceWithCategory); + std::unordered_map>& labelsCategoryMap); + void readLabels(const std::string& datatypeGroup, const std::string& uuid, std::vector& labelCategories, + std::vector>& labelsPerCategory, + std::vector& datumaroJsonPerCategory); + void writeLabels(const std::string& datatypeGroup, const std::string& uuid, + const std::vector& LabelsCategory); // ################ // Project // ################ @@ -273,11 +273,8 @@ class Hdf5CoreGeneral void getLabelCategories(std::string id, std::string labelType, std::vector& matchingLabelCategory); private: - void readLabel(const std::string& id, const std::string labelType, std::vector& labels); - void readlabelConfidences(const std::string& id, const std::string labelType, std::vector& labelConfidences); - void readBoundingBoxes(const std::string& id, const std::string boundingBoxType, - std::vector>& boundingBoxes); - void readInstances(const std::string& id, const std::string InstanceType, std::vector& instances); + template + T readDataset(const std::string& path); public: // header attribute keys @@ -303,13 +300,11 @@ class Hdf5CoreGeneral // dataset names inline static const std::string RAWDATA = "rawdata"; - inline static const std::string LABELGENERAL = "labelGeneral"; - inline static const std::string LABELGENERALCONFIDENCES = "labelGeneralConfidences"; - inline static const std::string LABELGENERALINSTANCES = "labelGeneralInstances"; - inline static const std::string LABELBB = "labelBB"; - inline static const std::string LABELBBCONFIDENCES = "labelBBConfidences"; - inline static const std::string LABELBBBOXESWITHROTATION = "labelBBBoxesWithRotation"; - inline static const std::string LABELBBINSTANCES = "labelBBInstances"; + inline static const std::string LABEL = "label"; + inline static const std::string LABEL_ID_DATUMARO = "labelIdDatumaro"; + inline static const std::string LABELINSTANCES = "labelInstances"; + inline static const std::string LABELINSTANCES_ID_DATUMARO = "labelInstancesIdDatumaro"; + inline static const std::string DATUMARO_JSON = "datumaroJson"; inline static const std::string POINTS = "points"; protected: diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h index 4a259e778..b1bb70ecc 100644 --- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h +++ b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h @@ -11,7 +11,7 @@ // seerep_msgs #include -#include +#include // std #include @@ -86,11 +86,9 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral, public Hdf5CoreDatatypeInt * @brief Write generals labels based on C++ data structures to HdF5 * * @param uuid uuid of the image data group - * @param labelsWithInstanceWithCategory vector of labels with instances in multiple categories + * @param labelCategory vector of labels with instances in multiple categories */ - void writeLabelsGeneral( - const std::string& uuid, - const std::vector& labelsWithInstanceWithCategory); + void writeLabels(const std::string& uuid, const std::vector& labelCategory); /** * @brief Write the general attributes of an image to hdf5 diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/impl/hdf5_core_general.hpp b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/impl/hdf5_core_general.hpp index 39b79ce2d..6ba8d5f6f 100644 --- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/impl/hdf5_core_general.hpp +++ b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/impl/hdf5_core_general.hpp @@ -88,4 +88,14 @@ void Hdf5CoreGeneral::readHeader(const std::string& id, HighFive::AnnotateTraits header.frameId = Hdf5CoreGeneral::readFrameId(object, HEADER_FRAME_ID, id); header.sequence = Hdf5CoreGeneral::readAttributeFromHdf5(object, HEADER_SEQ, id); } + +template +T Hdf5CoreGeneral::readDataset(const std::string& path) +{ + checkExists(path); + HighFive::DataSet datasetInstances = m_file->getDataSet(path); + T data; + datasetInstances.read(data); + return data; +} } // namespace seerep_hdf5_core diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp index 8276c2dd0..2ff577068 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_general.cpp @@ -79,144 +79,54 @@ const std::optional Hdf5CoreGeneral::readVersion() return version; } -void Hdf5CoreGeneral::readBoundingBoxLabeledAndAddToLabelsWithInstancesWithCategory( +void Hdf5CoreGeneral::readLabelsAndAddToLabelsPerCategory( const std::string& datatypeGroup, const std::string& uuid, - std::unordered_map>& labelsWithInstancesWithCategory) + std::unordered_map>& labelsCategoryMap) { - boost::uuids::string_generator gen; - - std::vector labelCategoriesBB; - std::vector> labelsBBPerCategory; - std::vector> labelBBConfidencesPerCategory; - std::vector>> boundingBoxesPerCategory; - std::vector> instancesPerCategory; - - readBoundingBoxLabeled(datatypeGroup, uuid, labelCategoriesBB, labelsBBPerCategory, labelBBConfidencesPerCategory, - boundingBoxesPerCategory, instancesPerCategory, false); - - // loop the label categories - for (std::size_t i = 0; i < labelCategoriesBB.size(); i++) - { - auto& labelsBB = labelsBBPerCategory.at(i); - auto& labelConfidenceBB = labelBBConfidencesPerCategory.at(i); - auto& instances = instancesPerCategory.at(i); - - // check if category already exists in map - // create new entry if it doesn't exist - auto labelsWithInstanceOfCategory = labelsWithInstancesWithCategory.find(labelCategoriesBB.at(i)); - if (labelsWithInstanceOfCategory == labelsWithInstancesWithCategory.end()) - { - auto emplaceResult = labelsWithInstancesWithCategory.emplace(labelCategoriesBB.at(i), - std::vector()); - labelsWithInstanceOfCategory = emplaceResult.first; - } - - // add labels with instance to this label category - for (std::size_t i = 0; i < labelsBB.size(); i++) - { - boost::uuids::uuid instanceUuid; - try - { - instanceUuid = gen(instances.at(i)); - } - catch (std::runtime_error&) - { - instanceUuid = boost::uuids::nil_uuid(); - } - labelsWithInstanceOfCategory->second.push_back(seerep_core_msgs::LabelWithInstance{ - .label = labelsBB.at(i), .labelConfidence = labelConfidenceBB.at(i), .uuidInstance = instanceUuid }); - } - } -} - -void Hdf5CoreGeneral::readBoundingBoxLabeled(const std::string& datatypeGroup, const std::string& uuid, - std::vector& labelCategories, - std::vector>& labelsPerCategory, - std::vector>& labelConfidencesPerCategory, - std::vector>>& boundingBoxesPerCategory, - std::vector>& instancesPerCategory, - bool loadBoxes) -{ - std::string id = datatypeGroup + "/" + uuid; - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "reading the bounding box with labels of " << id; - - getLabelCategories(id, LABELBB, labelCategories); - - labelsPerCategory.resize(labelCategories.size()); - labelConfidencesPerCategory.resize(labelCategories.size()); - boundingBoxesPerCategory.resize(labelCategories.size()); - instancesPerCategory.resize(labelCategories.size()); + std::vector labelCategories, datumaroJsonPerCategory; + std::vector> labelsPerCategory; + readLabels(datatypeGroup, uuid, labelCategories, labelsPerCategory, datumaroJsonPerCategory); for (std::size_t i = 0; i < labelCategories.size(); i++) { - readLabel(id, LABELBB + "_" + labelCategories.at(i), labelsPerCategory.at(i)); - readlabelConfidences(id, LABELBBCONFIDENCES + "_" + labelCategories.at(i), labelConfidencesPerCategory.at(i)); - readInstances(id, LABELBBINSTANCES + "_" + labelCategories.at(i), instancesPerCategory.at(i)); - - if (loadBoxes) - { - readBoundingBoxes(id, LABELBBBOXESWITHROTATION + "_" + labelCategories.at(i), boundingBoxesPerCategory.at(i)); - } - - if (labelsPerCategory.at(i).size() != labelConfidencesPerCategory.at(i).size() || - labelsPerCategory.at(i).size() != instancesPerCategory.at(i).size() || - (loadBoxes && labelsPerCategory.at(i).size() != boundingBoxesPerCategory.at(i).size())) - { - std::string errorMsg = "size of labels (" + std::to_string(labelsPerCategory.at(i).size()) + - "), size of confidences (" + std::to_string(labelConfidencesPerCategory.at(i).size()) + - +"), size of bounding boxes (" + std::to_string(boundingBoxesPerCategory.at(i).size()) + - ") and size of instances (" + std::to_string(instancesPerCategory.at(i).size()) + - ") do not fit."; - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << errorMsg; - throw std::runtime_error(errorMsg); - } - } -} - -void Hdf5CoreGeneral::readLabelsGeneralAndAddToLabelsWithInstancesWithCategory( - const std::string& datatypeGroup, const std::string& uuid, - std::unordered_map>& labelsWithInstancesWithCategory) -{ - std::vector labelCategoriesGeneral; - std::vector> labelsWithInstancesGeneralPerCategory; - readLabelsGeneral(datatypeGroup, uuid, labelCategoriesGeneral, labelsWithInstancesGeneralPerCategory); - - for (std::size_t i = 0; i < labelCategoriesGeneral.size(); i++) - { - labelsWithInstancesWithCategory.emplace(labelCategoriesGeneral.at(i), labelsWithInstancesGeneralPerCategory.at(i)); + labelsCategoryMap.emplace(labelCategories.at(i), labelsPerCategory.at(i)); } } -void Hdf5CoreGeneral::readLabelsGeneral( - const std::string& datatypeGroup, const std::string& uuid, std::vector& labelCategories, - std::vector>& labelsWithInstancesGeneralPerCategory) +void Hdf5CoreGeneral::readLabels(const std::string& datatypeGroup, const std::string& uuid, + std::vector& labelCategories, + std::vector>& labelsPerCategory, + std::vector& datumaroJsonPerCategory) { boost::uuids::string_generator gen; std::string id = datatypeGroup + "/" + uuid; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading labels general of " << id; - getLabelCategories(id, LABELGENERAL, labelCategories); + getLabelCategories(id, LABEL, labelCategories); for (std::string category : labelCategories) { - std::vector labels, instances; - std::vector labelConfidences; - readLabel(id, LABELGENERAL + "_" + category, labels); - readlabelConfidences(id, LABELGENERALCONFIDENCES + "_" + category, labelConfidences); - readInstances(id, LABELGENERALINSTANCES + "_" + category, instances); - - if (labels.size() != labelConfidences.size() || labels.size() != instances.size()) + std::vector labels = readDataset>(id + "/" + LABEL + "_" + category); + std::vector labelsIdDatumaro = readDataset>(id + "/" + LABEL_ID_DATUMARO + "_" + category); + std::vector instances = + readDataset>(id + "/" + LABELINSTANCES + "_" + category); + std::vector instancesIdDatumaro = + readDataset>(id + "/" + LABELINSTANCES_ID_DATUMARO + "_" + category); + datumaroJsonPerCategory.push_back(readDataset(id + "/" + DATUMARO_JSON + "_" + category)); + + if (labels.size() != labelsIdDatumaro.size() || labels.size() != instances.size() || + labels.size() != instancesIdDatumaro.size()) { - std::string errorMsg = "size of labels (" + std::to_string(labels.size()) + ") and size of confidences (" + - std::to_string(labelConfidences.size()) + ") and size of instances (" + - std::to_string(instances.size()) + ") do not fit."; + std::string errorMsg = "size of labels (" + std::to_string(labels.size()) + ") and size of label datumaro ids (" + + std::to_string(labelsIdDatumaro.size()) + ") and size of instances (" + + std::to_string(instances.size()) + ") and size of instances datumaro ids (" + + std::to_string(instancesIdDatumaro.size()) + ") do not fit."; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << errorMsg; throw std::runtime_error(errorMsg); } - std::vector labelsWithInstancesGeneral; + std::vector label; for (long unsigned int i = 0; i < labels.size(); i++) { boost::uuids::uuid instanceUuid; @@ -228,37 +138,48 @@ void Hdf5CoreGeneral::readLabelsGeneral( { instanceUuid = boost::uuids::nil_uuid(); } - labelsWithInstancesGeneral.push_back(seerep_core_msgs::LabelWithInstance{ - .label = labels.at(i), .labelConfidence = labelConfidences.at(i), .uuidInstance = instanceUuid }); + label.push_back(seerep_core_msgs::Label{ .label = labels.at(i), + .labelIdDatumaro = labelsIdDatumaro.at(i), + .uuidInstance = instanceUuid, + .instanceIdDatumaro = instancesIdDatumaro.at(i) }); } - labelsWithInstancesGeneralPerCategory.push_back(labelsWithInstancesGeneral); + labelsPerCategory.push_back(label); } } -void Hdf5CoreGeneral::writeLabelsGeneral( - const std::string& datatypeGroup, const std::string& uuid, - const std::vector& labelsWithInstanceWithCategory) +void Hdf5CoreGeneral::writeLabels(const std::string& datatypeGroup, const std::string& uuid, + const std::vector& LabelCategory) { - for (auto labels : labelsWithInstanceWithCategory) + std::string id = datatypeGroup + "/" + uuid; + for (auto labels : LabelCategory) { - std::string id = datatypeGroup + "/" + uuid; - - HighFive::DataSet datasetLabels = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELGENERAL + "_" + labels.category, - HighFive::DataSpace::From(labels.labels)); + HighFive::DataSet datasetLabels = + m_file->createDataSet(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABEL + "_" + labels.category, + HighFive::DataSpace::From(labels.labels)); datasetLabels.write(labels.labels); - HighFive::DataSet datasetLabelConfidences = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELGENERALCONFIDENCES + "_" + labels.category, - HighFive::DataSpace::From(labels.labelConfidences)); - datasetLabelConfidences.write(labels.labelConfidences); + HighFive::DataSet datasetLabelsIdDatumaro = m_file->createDataSet( + id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABEL_ID_DATUMARO + "_" + labels.category, + HighFive::DataSpace::From(labels.labelsIdDatumaro)); + datasetLabelsIdDatumaro.write(labels.labelsIdDatumaro); HighFive::DataSet datasetInstances = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELGENERALINSTANCES + "_" + labels.category, + id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELINSTANCES + "_" + labels.category, HighFive::DataSpace::From(labels.instances)); datasetInstances.write(labels.instances); + + HighFive::DataSet datasetInstancesIdDatumaro = m_file->createDataSet( + id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELINSTANCES_ID_DATUMARO + "_" + labels.category, + HighFive::DataSpace::From(labels.instancesIdDatumaro)); + datasetInstancesIdDatumaro.write(labels.instancesIdDatumaro); + + HighFive::DataSet datasetDatumaroJson = m_file->createDataSet( + id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::DATUMARO_JSON + "_" + labels.category, + HighFive::DataSpace::From(labels.datumaroJson)); + datasetDatumaroJson.write(labels.datumaroJson); } + m_file->flush(); } @@ -352,34 +273,6 @@ bool Hdf5CoreGeneral::exists(const std::string& path) const return true; } -void Hdf5CoreGeneral::readLabel(const std::string& id, const std::string labelType, std::vector& labels) -{ - checkExists(id + "/" + labelType); - HighFive::DataSet datasetLabels = m_file->getDataSet(id + "/" + labelType); - datasetLabels.read(labels); -} -void Hdf5CoreGeneral::readlabelConfidences(const std::string& id, const std::string labelConfidencesType, - std::vector& labelConfidences) -{ - checkExists(id + "/" + labelConfidencesType); - HighFive::DataSet datasetLabels = m_file->getDataSet(id + "/" + labelConfidencesType); - datasetLabels.read(labelConfidences); -} -void Hdf5CoreGeneral::readBoundingBoxes(const std::string& id, const std::string boundingBoxType, - std::vector>& boundingBoxes) -{ - checkExists(id + "/" + boundingBoxType); - HighFive::DataSet datasetBoxes = m_file->getDataSet(id + "/" + boundingBoxType); - datasetBoxes.read(boundingBoxes); -} -void Hdf5CoreGeneral::readInstances(const std::string& id, const std::string instanceType, - std::vector& instances) -{ - checkExists(id + "/" + instanceType); - HighFive::DataSet datasetInstances = m_file->getDataSet(id + "/" + instanceType); - datasetInstances.read(instances); -} - std::shared_ptr Hdf5CoreGeneral::getHdf5Group(const std::string& group_path, bool create) { if (exists(group_path)) diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp index fa851c6c2..2f7f83a9f 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp @@ -42,11 +42,7 @@ std::optional Hdf5CoreImage::readDataset(con boost::uuids::uuid uuid_generated = gen(uuid); data.header.uuidData = uuid_generated; - readLabelsGeneralAndAddToLabelsWithInstancesWithCategory(HDF5_GROUP_IMAGE, uuid, - data.labelsWithInstancesWithCategory); - - readBoundingBoxLabeledAndAddToLabelsWithInstancesWithCategory(HDF5_GROUP_IMAGE, uuid, - data.labelsWithInstancesWithCategory); + readLabelsAndAddToLabelsPerCategory(HDF5_GROUP_IMAGE, uuid, data.labelsCategory); // fetch cam intrinsics uuid from hdf5_core_cameraintrinsics camintrinsics_uuid = readAttributeFromHdf5( @@ -64,12 +60,10 @@ std::vector Hdf5CoreImage::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_IMAGE); } -void Hdf5CoreImage::writeLabelsGeneral( - const std::string& uuid, - const std::vector& labelsWithInstanceWithCategory) +void Hdf5CoreImage::writeLabels(const std::string& uuid, + const std::vector& labelCategory) { - Hdf5CoreGeneral::writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, uuid, - labelsWithInstanceWithCategory); + Hdf5CoreGeneral::writeLabels(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, uuid, labelCategory); } void Hdf5CoreImage::writeImageAttributes(const std::string& id, const ImageAttributes& attributes) diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp index 38da1b543..ff8203c81 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp @@ -48,7 +48,7 @@ std::optional Hdf5CorePoint::readDataset(con data.boundingbox.max_corner().set<1>(read_data.at(1)); data.boundingbox.max_corner().set<2>(read_data.at(2)); - readLabelsGeneralAndAddToLabelsWithInstancesWithCategory(HDF5_GROUP_POINT, uuid, data.labelsWithInstancesWithCategory); + readLabelsAndAddToLabelsPerCategory(HDF5_GROUP_POINT, uuid, data.labelsCategory); return data; } diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp index 0a2dac1e8..b39f6b48b 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp @@ -45,11 +45,7 @@ std::optional Hdf5CorePointCloud::readDatase data.boundingbox.max_corner().set<1>(bb.at(4)); data.boundingbox.max_corner().set<2>(bb.at(5)); - readLabelsGeneralAndAddToLabelsWithInstancesWithCategory(HDF5_GROUP_POINTCLOUD, uuid, - data.labelsWithInstancesWithCategory); - - readBoundingBoxLabeledAndAddToLabelsWithInstancesWithCategory(HDF5_GROUP_POINTCLOUD, uuid, - data.labelsWithInstancesWithCategory); + readLabelsAndAddToLabelsPerCategory(HDF5_GROUP_POINTCLOUD, uuid, data.labelsCategory); return data; } diff --git a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_general.h b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_general.h index fcc8f7670..e89156199 100644 --- a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_general.h +++ b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_general.h @@ -8,12 +8,8 @@ #include // seerep-msgs -#include -#include -#include -#include #include -#include +#include #include // grpc / flatbuffer @@ -32,14 +28,8 @@ namespace seerep_hdf5_fb { // make nested flatbuffers readable -typedef flatbuffers::Vector> BoundingBoxesLabeledFb; -typedef flatbuffers::Vector> - BoundingBoxesLabeledWithCategoryFb; -typedef flatbuffers::Vector> BoundingBoxes2dLabeledFb; -typedef flatbuffers::Vector> - BoundingBoxes2dLabeledWithCategoryFb; -typedef flatbuffers::Vector> GeneralLabelsFb; -typedef flatbuffers::Vector> GeneralLabelsWithCategoryFb; +typedef flatbuffers::Vector> LabelsFb; +typedef flatbuffers::Vector> LabelsCategoryFb; typedef flatbuffers::Vector> AttributeMapsFb; /** @@ -97,76 +87,14 @@ class Hdf5FbGeneral : public virtual seerep_hdf5_core::Hdf5CoreGeneral HighFive::AnnotateTraits& object, std::string uuidMsg); - /** - * @brief Write a flatbuffers 3D bounding box message with category to hdf5 - * - * @param datatypeGroup the data type the bounding box should be written to e.g point cloud, image - * @param uuid the uuid of the data group, the bounding box should be written to - * @param boundingBoxLabeledWithCategory the flatbuffers 3D bounding box with category message - */ - void writeBoundingBoxLabeled(const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxesLabeledWithCategoryFb* boundingBoxLabeledWithCategory); - /** - * @brief Write a flatbuffers 3D bounding box message with category to hdf5 - * - * @param datatypeGroup the data type the bounding box should be written to e.g point cloud, image - * @param uuid the uuid of the data group, the bounding box should be written to - * @param boundingboxLabeled the flatbuffers 3D bounding box message - * @param category the category those bounding boxes are in - */ - void writeBoundingBoxLabeled(const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxesLabeledFb* boundingboxLabeled, const std::string& category); - /** - * @brief Read the bounding boxes of all categories from hdf5 and receive it as a flatbuffers message - * - * @param datatypeGroup the data type the bounding box should be written to e.g image - * @param uuid uuid of the data group, the bounding box should be written to - * @param builder the flatbuffers message builder - * @return flatbuffers::Offset - */ - flatbuffers::Offset - readBoundingBoxesLabeled(const std::string& datatypeGroup, const std::string& uuid, - flatbuffers::grpc::MessageBuilder& builder); - /** - * @brief Write a flatbuffers 2D bounding box message to hdf5 - * - * @param datatypeGroup the data type the bounding box should be written to e.g image - * @param uuid the uuid of the data group, the bounding box should be written to - * @param boundingbox2dLabeled the flatbuffers 2D bounding box message - */ - void writeBoundingBox2DLabeled(const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxes2dLabeledWithCategoryFb* boundingbox2DLabeledWithCategoryVector); - - /** - * @brief Write a flatbuffers 2D bounding box message to hdf5 - * - * @param datatypeGroup the data type the bounding box should be written to e.g image - * @param uuid the uuid of the data group, the bounding box should be written to - * @param boundingbox2dLabeled the flatbuffers 2D bounding box message - */ - void writeBoundingBox2DLabeled(const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxes2dLabeledFb* boundingbox2DLabeled, const std::string& category); - /** - * @brief Read the 2D bounding boxes of all categories from hdf5 and receive it as a flatbuffers message - * - * @param datatypeGroup the data type the bounding box should be written to e.g image - * @param uuid uuid of the data group, the bounding box should be written to - * @param builder the flatbuffers message builder - * @return flatbuffers::Offset the flatbuffers 2D bounding box message - */ - flatbuffers::Offset - readBoundingBoxes2DLabeled(const std::string& datatypeGroup, const std::string& uuid, - flatbuffers::grpc::MessageBuilder& builder); - /** * @brief Write a flatbuffers general labels message to hdf5 * * @param datatypeGroup the data type the general labels should be written to e.g point cloud, image * @param uuid the uuid of the data group, the general labels should be written to - * @param labelsGeneral the flatbuffers general labels message + * @param labels the flatbuffers general labels message */ - void writeLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, - const GeneralLabelsWithCategoryFb* labelsGeneral); + void writeLabels(const std::string& datatypeGroup, const std::string& uuid, const LabelsCategoryFb* labels); /** * @brief Read general labels (with instances and of all categories) from hdf5 and receive it as a flatbuffers message * @@ -176,9 +104,8 @@ class Hdf5FbGeneral : public virtual seerep_hdf5_core::Hdf5CoreGeneral * @return flatbuffers::Offset the flatbuffers general labels message (with instances and * of all categories) */ - flatbuffers::Offset readGeneralLabels(const std::string& datatypeGroup, - const std::string& uuid, - flatbuffers::grpc::MessageBuilder& builder); + flatbuffers::Offset readLabels(const std::string& datatypeGroup, const std::string& uuid, + flatbuffers::grpc::MessageBuilder& builder); }; } // namespace seerep_hdf5_fb diff --git a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_image.h b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_image.h index 279735645..a712afd85 100644 --- a/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_image.h +++ b/seerep_hdf5/seerep_hdf5_fb/include/seerep_hdf5_fb/hdf5_fb_image.h @@ -50,18 +50,6 @@ class Hdf5FbImage : public Hdf5FbGeneral, public seerep_hdf5_core::Hdf5CoreImage */ void writeImage(const std::string& id, const seerep::fb::Image& image); - /** - * @brief Write a BoundingBoxes2D flatbuffers message to hdf5 - * - * Currently only used by the ROS dumper - * - * @param id the uuid of the image data group - * @param bb2DLabeledWithCategory the flatbuffers BoundingBoxes2D with category message - */ - void writeImageBoundingBox2DLabeled( - const std::string& id, - const flatbuffers::Vector>* - bb2DLabeledWithCategory); /** * @brief Read an image from hdf5 and receive it as a flatbuffers message * 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 d1eeca875..34a1455cc 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 @@ -61,15 +61,6 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void writeBoundingBox(const std::string& uuid, const seerep_core_msgs::Point& min_corner, const seerep_core_msgs::Point& max_corner); - /** - * @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 3bdbcf9a4..f4b983170 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_general.cpp @@ -49,380 +49,85 @@ void Hdf5FbGeneral::writeAttributeMap( } } -void Hdf5FbGeneral::writeBoundingBoxLabeled( - const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxesLabeledWithCategoryFb* boundingBoxLabeledWithCategoryVector) +void Hdf5FbGeneral::writeLabels(const std::string& datatypeGroup, const std::string& uuid, + const LabelsCategoryFb* labelCategoryVectorFb) { - if (boundingBoxLabeledWithCategoryVector && boundingBoxLabeledWithCategoryVector->size() > 0) + if (labelCategoryVectorFb && labelCategoryVectorFb->size() != 0) { - for (auto boundingBoxLabeledWithCategory : *boundingBoxLabeledWithCategoryVector) + std::vector labelCategoryVector; + for (auto labelsCategory : *labelCategoryVectorFb) { - writeBoundingBoxLabeled(datatypeGroup, uuid, boundingBoxLabeledWithCategory->boundingBoxLabeled(), - boundingBoxLabeledWithCategory->category()->c_str()); - } - } -} - -void Hdf5FbGeneral::writeBoundingBoxLabeled(const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxesLabeledFb* boundingboxLabeled, - const std::string& category) -{ - if (boundingboxLabeled && boundingboxLabeled->size() != 0) - { - std::string id = datatypeGroup + "/" + uuid; - - std::vector labels; - std::vector labelConfidence; - std::vector> boundingBoxesWithRotation; - std::vector instances; - for (auto label : *boundingboxLabeled) - { - labels.push_back(label->labelWithInstance()->label()->label()->str()); - labelConfidence.push_back(label->labelWithInstance()->label()->confidence()); - - double rotX = 0.0, rotY = 0.0, rotZ = 0.0, rotW = 1.0; - if (label->bounding_box()->rotation() != NULL) + std::vector labels, instances; + std::vector labelsIdDatumaro, instancesIdDatumaro; + for (auto label : *labelsCategory->labels()) { - 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()); - } - - HighFive::DataSet datasetLabels = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBB + "_" + category, HighFive::DataSpace::From(labels)); - datasetLabels.write(labels); - - HighFive::DataSet datasetLabelConfidences = - m_file->createDataSet(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBCONFIDENCES + "_" + category, - HighFive::DataSpace::From(labelConfidence)); - datasetLabelConfidences.write(labelConfidence); - - HighFive::DataSet datasetBoxes = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBBOXESWITHROTATION + "_" + category, - HighFive::DataSpace::From(boundingBoxesWithRotation)); - datasetBoxes.write(boundingBoxesWithRotation); - - HighFive::DataSet datasetInstances = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBINSTANCES + "_" + category, - HighFive::DataSpace::From(instances)); - datasetInstances.write(instances); - - m_file->flush(); - } -} - -void Hdf5FbGeneral::writeBoundingBox2DLabeled( - const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxes2dLabeledWithCategoryFb* boundingbox2DLabeledWithCategoryVector) -{ - if (boundingbox2DLabeledWithCategoryVector && boundingbox2DLabeledWithCategoryVector->size() > 0) - { - for (auto boundingBox2DLabeledWithCategory : *boundingbox2DLabeledWithCategoryVector) - { - writeBoundingBox2DLabeled(datatypeGroup, uuid, boundingBox2DLabeledWithCategory->boundingBox2dLabeled(), - boundingBox2DLabeledWithCategory->category()->c_str()); - } - } -} - -void Hdf5FbGeneral::writeBoundingBox2DLabeled(const std::string& datatypeGroup, const std::string& uuid, - const BoundingBoxes2dLabeledFb* boundingbox2DLabeled, - const std::string& category) -{ - std::string id = datatypeGroup + "/" + uuid; - - if (boundingbox2DLabeled && boundingbox2DLabeled->size() != 0) - { - std::vector labels; - std::vector labelConfidences; - std::vector> boundingBoxesWithRotation; - std::vector instances; - for (auto label : *boundingbox2DLabeled) - { - labels.push_back(label->labelWithInstance()->label()->label()->str()); - labelConfidences.push_back(label->labelWithInstance()->label()->confidence()); - std::vector boxWithRotation{ label->bounding_box()->center_point()->x(), - label->bounding_box()->center_point()->y(), - label->bounding_box()->spatial_extent()->x(), - label->bounding_box()->spatial_extent()->y(), - label->bounding_box()->rotation() }; - boundingBoxesWithRotation.push_back(boxWithRotation); - - instances.push_back(label->labelWithInstance()->instanceUuid()->str()); - } - - if (m_file->exist(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBB + "_" + category) || - m_file->exist(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBCONFIDENCES + "_" + category) || - m_file->exist(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBBOXESWITHROTATION + "_" + category) || - m_file->exist(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBINSTANCES + "_" + category)) - { - throw std::invalid_argument(datatypeGroup + " " + uuid + " already has bounding box based labels"); - } - else - { - HighFive::DataSet datasetLabels = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBB + "_" + category, HighFive::DataSpace::From(labels)); - datasetLabels.write(labels); - - HighFive::DataSet datasetLabelConfidences = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBCONFIDENCES + "_" + category, - HighFive::DataSpace::From(labelConfidences)); - datasetLabelConfidences.write(labelConfidences); - - HighFive::DataSet datasetBoxes = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBBOXESWITHROTATION + "_" + category, - HighFive::DataSpace::From(boundingBoxesWithRotation)); - datasetBoxes.write(boundingBoxesWithRotation); - - HighFive::DataSet datasetInstances = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBINSTANCES + "_" + category, - HighFive::DataSpace::From(instances)); - datasetInstances.write(instances); - } - - m_file->flush(); - } -} - -void Hdf5FbGeneral::writeLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, - const GeneralLabelsWithCategoryFb* labelsWithInstanceWithCategoryGeneralVector) -{ - if (labelsWithInstanceWithCategoryGeneralVector && labelsWithInstanceWithCategoryGeneralVector->size() != 0) - { - std::vector labelsWithCategoryVector; - for (auto labelsWithInstanceWithCategoryGeneral : *labelsWithInstanceWithCategoryGeneralVector) - { - std::vector labels; - std::vector labelConfidences; - std::vector instances; - for (auto label : *labelsWithInstanceWithCategoryGeneral->labelsWithInstance()) - { - labels.push_back(label->label()->label()->str()); - labelConfidences.push_back(label->label()->confidence()); - + labels.push_back(label->label()->str()); + labelsIdDatumaro.push_back(label->labelIdDatumaro()); instances.push_back(label->instanceUuid()->str()); + instancesIdDatumaro.push_back(label->instanceIdDatumaro()); } - seerep_core_msgs::LabelsWithInstanceWithCategory labelsWithCategory; - labelsWithCategory.category = labelsWithInstanceWithCategoryGeneral->category()->c_str(); - labelsWithCategory.labels = labels; - labelsWithCategory.labelConfidences = labelConfidences; - labelsWithCategory.instances = instances; - labelsWithCategoryVector.push_back(labelsWithCategory); + seerep_core_msgs::LabelCategory labelCategory; + labelCategory.category = labelsCategory->category()->c_str(); + labelCategory.labels = labels; + labelCategory.labelsIdDatumaro = labelsIdDatumaro; + labelCategory.instances = instances; + labelCategory.instancesIdDatumaro = instancesIdDatumaro; + labelCategory.datumaroJson = labelsCategory->datumaroJson()->c_str(); + labelCategoryVector.push_back(labelCategory); } - Hdf5CoreGeneral::writeLabelsGeneral(datatypeGroup, uuid, labelsWithCategoryVector); + Hdf5CoreGeneral::writeLabels(datatypeGroup, uuid, labelCategoryVector); } } -flatbuffers::Offset -Hdf5FbGeneral::readGeneralLabels(const std::string& datatypeGroup, const std::string& uuid, - flatbuffers::grpc::MessageBuilder& builder) +flatbuffers::Offset Hdf5FbGeneral::readLabels(const std::string& datatypeGroup, + const std::string& uuid, + flatbuffers::grpc::MessageBuilder& builder) { - std::vector labelCategories; - std::vector> labelsWithInstancesGeneralPerCategory; + std::vector labelCategories, datumaroJsonPerCategory; + std::vector> labelsCategory; - seerep_hdf5_core::Hdf5CoreGeneral::readLabelsGeneral(datatypeGroup, uuid, labelCategories, - labelsWithInstancesGeneralPerCategory); + seerep_hdf5_core::Hdf5CoreGeneral::readLabels(datatypeGroup, uuid, labelCategories, labelsCategory, + datumaroJsonPerCategory); - std::vector> labelsWithInstanceWithCategoryVector; - labelsWithInstanceWithCategoryVector.reserve(labelCategories.size()); + std::vector> labelCategory; + labelCategory.reserve(labelCategories.size()); // loop categories for (size_t icat = 0; icat < labelCategories.size(); icat++) { - std::vector> labelGeneralVector; - labelGeneralVector.reserve(labelsWithInstancesGeneralPerCategory.at(icat).size()); + std::vector> labelVector; + labelVector.reserve(labelsCategory.at(icat).size()); // loop labels - for (size_t i = 0; i < labelsWithInstancesGeneralPerCategory.at(icat).size(); i++) + for (size_t i = 0; i < labelsCategory.at(icat).size(); i++) { - auto labelStr = builder.CreateString(labelsWithInstancesGeneralPerCategory.at(icat).at(i).label); - auto instanceOffset = builder.CreateString( - boost::lexical_cast(labelsWithInstancesGeneralPerCategory.at(icat).at(i).uuidInstance)); + auto labelStr = builder.CreateString(labelsCategory.at(icat).at(i).label); + auto instanceOffset = + builder.CreateString(boost::lexical_cast(labelsCategory.at(icat).at(i).uuidInstance)); seerep::fb::LabelBuilder labelBuilder(builder); labelBuilder.add_label(labelStr); - labelBuilder.add_confidence(labelsWithInstancesGeneralPerCategory.at(icat).at(i).labelConfidence); - auto labelMsg = labelBuilder.Finish(); - - seerep::fb::LabelWithInstanceBuilder labelInstanceBuilder(builder); - labelInstanceBuilder.add_label(labelMsg); - labelInstanceBuilder.add_instanceUuid(instanceOffset); - labelGeneralVector.push_back(labelInstanceBuilder.Finish()); + labelBuilder.add_labelIdDatumaro(labelsCategory.at(icat).at(i).labelIdDatumaro); + labelBuilder.add_instanceUuid(instanceOffset); + labelBuilder.add_instanceIdDatumaro(labelsCategory.at(icat).at(i).instanceIdDatumaro); + labelVector.push_back(labelBuilder.Finish()); } - auto labelsGeneralOfCategoryOffset = - builder.CreateVector>(labelGeneralVector); + auto labelOffset = builder.CreateVector>(labelVector); auto categoryOffset = builder.CreateString(labelCategories.at(icat)); - seerep::fb::LabelsWithInstanceWithCategoryBuilder labelsWithInstanceWithCategoryBuilder(builder); - labelsWithInstanceWithCategoryBuilder.add_category(categoryOffset); - labelsWithInstanceWithCategoryBuilder.add_labelsWithInstance(labelsGeneralOfCategoryOffset); - auto labelsOfCategory = labelsWithInstanceWithCategoryBuilder.Finish(); - labelsWithInstanceWithCategoryVector.push_back(labelsOfCategory); - } - - return builder.CreateVector>( - labelsWithInstanceWithCategoryVector); -} - -flatbuffers::Offset -Hdf5FbGeneral::readBoundingBoxes2DLabeled(const std::string& datatypeGroup, const std::string& uuid, - flatbuffers::grpc::MessageBuilder& builder) -{ - std::vector labelCategories; - std::vector> labelsPerCategory; - std::vector> labelConfidencesPerCategory; - std::vector>> boundingBoxesPerCategory; - std::vector> instancesPerCategory; - readBoundingBoxLabeled(datatypeGroup, uuid, labelCategories, labelsPerCategory, labelConfidencesPerCategory, - boundingBoxesPerCategory, instancesPerCategory); - - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "creating the bounding boxes 2d with label fb msgs"; - - std::vector> boundingBox2DLabeledVector; - boundingBox2DLabeledVector.reserve(labelCategories.size()); - - // loop categories - for (size_t icat = 0; icat < labelCategories.size(); icat++) - { - std::vector> bblabeledVector; - bblabeledVector.reserve(labelsPerCategory.at(icat).size()); - // loop labels - for (size_t i = 0; i < labelsPerCategory.at(icat).size(); i++) - { - auto InstanceOffset = builder.CreateString(instancesPerCategory.at(icat).at(i)); - auto labelStr = builder.CreateString(labelsPerCategory.at(icat).at(i)); - - seerep::fb::LabelBuilder labelBuilder(builder); - labelBuilder.add_label(labelStr); - labelBuilder.add_confidence(labelConfidencesPerCategory.at(icat).at(i)); - auto labelMsg = labelBuilder.Finish(); - - seerep::fb::LabelWithInstanceBuilder labelInstanceBuilder(builder); - labelInstanceBuilder.add_instanceUuid(InstanceOffset); - labelInstanceBuilder.add_label(labelMsg); - auto labelWithInstanceOffset = labelInstanceBuilder.Finish(); - - auto centerPoint = seerep::fb::CreatePoint2D(builder, boundingBoxesPerCategory.at(icat).at(i).at(0), - boundingBoxesPerCategory.at(icat).at(i).at(1)); - auto spatialExtent = seerep::fb::CreatePoint2D(builder, boundingBoxesPerCategory.at(icat).at(i).at(2), - boundingBoxesPerCategory.at(icat).at(i).at(3)); + auto datumaroJsonOffset = builder.CreateString(datumaroJsonPerCategory.at(icat)); - seerep::fb::Boundingbox2DBuilder bbBuilder(builder); - bbBuilder.add_center_point(centerPoint); - bbBuilder.add_spatial_extent(spatialExtent); - bbBuilder.add_rotation(boundingBoxesPerCategory.at(icat).at(i).at(4)); - auto bb = bbBuilder.Finish(); - - seerep::fb::BoundingBox2DLabeledBuilder bblabeledBuilder(builder); - bblabeledBuilder.add_bounding_box(bb); - bblabeledBuilder.add_labelWithInstance(labelWithInstanceOffset); - - bblabeledVector.push_back(bblabeledBuilder.Finish()); - } - auto bblabeledVectorOffset = builder.CreateVector(bblabeledVector); - auto categoryOffset = builder.CreateString(labelCategories.at(icat)); + seerep::fb::LabelCategoryBuilder labelsCategoryBuilder(builder); + labelsCategoryBuilder.add_category(categoryOffset); + labelsCategoryBuilder.add_labels(labelOffset); + labelsCategoryBuilder.add_datumaroJson(datumaroJsonOffset); - seerep::fb::BoundingBox2DLabeledWithCategoryBuilder boundingBox2DLabeledWithCategoryBuilder(builder); - boundingBox2DLabeledWithCategoryBuilder.add_category(categoryOffset); - boundingBox2DLabeledWithCategoryBuilder.add_boundingBox2dLabeled(bblabeledVectorOffset); - auto boundingBox2DLabeledWithCategory = boundingBox2DLabeledWithCategoryBuilder.Finish(); - boundingBox2DLabeledVector.push_back(boundingBox2DLabeledWithCategory); + auto labelsOfCategory = labelsCategoryBuilder.Finish(); + labelCategory.push_back(labelsOfCategory); } - return builder.CreateVector>( - boundingBox2DLabeledVector); -} - -flatbuffers::Offset -Hdf5FbGeneral::readBoundingBoxesLabeled(const std::string& datatypeGroup, const std::string& uuid, - flatbuffers::grpc::MessageBuilder& builder) -{ - std::vector labelCategories; - std::vector> labelsPerCategory; - std::vector> labelConfidencesPerCategory; - std::vector>> boundingBoxesPerCategory; - std::vector> instancesPerCategory; - readBoundingBoxLabeled(datatypeGroup, uuid, labelCategories, labelsPerCategory, labelConfidencesPerCategory, - boundingBoxesPerCategory, instancesPerCategory); - - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "creating the bounding boxes 3d with label fb msgs"; - - std::vector> boundingBoxLabeledVector; - boundingBoxLabeledVector.reserve(labelCategories.size()); - - // loop categories - for (size_t icat = 0; icat < labelCategories.size(); icat++) - { - std::vector> bblabeledVector; - bblabeledVector.reserve(labelsPerCategory.at(icat).size()); - // loop labels - for (size_t i = 0; i < labelsPerCategory.at(icat).size(); i++) - { - auto InstanceOffset = builder.CreateString(instancesPerCategory.at(icat).at(i)); - auto labelStr = builder.CreateString(labelsPerCategory.at(icat).at(i)); - seerep::fb::LabelBuilder labelBuilder(builder); - labelBuilder.add_label(labelStr); - labelBuilder.add_confidence(labelConfidencesPerCategory.at(icat).at(i)); - auto labelMsg = labelBuilder.Finish(); - - seerep::fb::LabelWithInstanceBuilder labelInstanceBuilder(builder); - labelInstanceBuilder.add_instanceUuid(InstanceOffset); - labelInstanceBuilder.add_label(labelMsg); - auto labelWithInstanceOffset = labelInstanceBuilder.Finish(); - - auto centerPoint = seerep::fb::CreatePoint(builder, boundingBoxesPerCategory.at(icat).at(i).at(0), - boundingBoxesPerCategory.at(icat).at(i).at(1), - boundingBoxesPerCategory.at(icat).at(i).at(2)); - auto spatialExtent = seerep::fb::CreatePoint(builder, boundingBoxesPerCategory.at(icat).at(i).at(3), - boundingBoxesPerCategory.at(icat).at(i).at(4), - boundingBoxesPerCategory.at(icat).at(i).at(5)); - - auto rotation = seerep::fb::CreateQuaternion(builder, boundingBoxesPerCategory.at(icat).at(i).at(6), - boundingBoxesPerCategory.at(icat).at(i).at(7), - boundingBoxesPerCategory.at(icat).at(i).at(8), - boundingBoxesPerCategory.at(icat).at(i).at(9)); - - seerep::fb::BoundingboxBuilder bbBuilder(builder); - bbBuilder.add_center_point(centerPoint); - bbBuilder.add_spatial_extent(spatialExtent); - bbBuilder.add_rotation(rotation); - auto bb = bbBuilder.Finish(); - - seerep::fb::BoundingBoxLabeledBuilder bblabeledBuilder(builder); - bblabeledBuilder.add_bounding_box(bb); - bblabeledBuilder.add_labelWithInstance(labelWithInstanceOffset); - - bblabeledVector.push_back(bblabeledBuilder.Finish()); - } - auto bblabeledVectorOffset = builder.CreateVector(bblabeledVector); - auto categoryOffset = builder.CreateString(labelCategories.at(icat)); - - seerep::fb::BoundingBoxLabeledWithCategoryBuilder boundingBoxLabeledWithCategoryBuilder(builder); - boundingBoxLabeledWithCategoryBuilder.add_category(categoryOffset); - boundingBoxLabeledWithCategoryBuilder.add_boundingBoxLabeled(bblabeledVectorOffset); - auto boundingBoxLabeledWithCategory = boundingBoxLabeledWithCategoryBuilder.Finish(); - boundingBoxLabeledVector.push_back(boundingBoxLabeledWithCategory); - } - return builder.CreateVector>(boundingBoxLabeledVector); + return builder.CreateVector>(labelCategory); } } // namespace seerep_hdf5_fb diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_image.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_image.cpp index 93e44e1a6..e260abf32 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_image.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_image.cpp @@ -31,23 +31,12 @@ void Hdf5FbImage::writeImage(const std::string& id, const seerep::fb::Image& ima // use pointers from start and end of the array as iterators dataSetPtr->write(std::vector(arrayStartPtr, arrayStartPtr + image.data()->size())); - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_bb()); // name is currently ambiguous, use fully qualified name - seerep_hdf5_fb::Hdf5FbGeneral::writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, - image.labels_general()); + seerep_hdf5_fb::Hdf5FbGeneral::writeLabels(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels()); m_file->flush(); } -void Hdf5FbImage::writeImageBoundingBox2DLabeled( - const std::string& id, - const flatbuffers::Vector>* bb2DLabeledWithCategory) -{ - const std::scoped_lock lock(*m_write_mtx); - - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, bb2DLabeledWithCategory); -} - std::optional> Hdf5FbImage::readImage(const std::string& id, const bool withoutData) { @@ -76,9 +65,7 @@ std::optional> Hdf5FbImage::readIm auto dataGroupPtr = getHdf5Group(hdf5GroupPath); auto headerOffset = readHeaderAttributes(builder, *dataGroupPtr, id); - auto boxes2DLabeledOffset = - readBoundingBoxes2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, builder); - auto generalLabelsOffset = readGeneralLabels(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, builder); + auto labelsOffset = readLabels(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, builder); auto encodingStringOffset = builder.CreateString(imageAttributes.encoding); std::vector data; @@ -107,8 +94,7 @@ std::optional> Hdf5FbImage::readIm imageBuilder.add_data(imageDataOffset); } imageBuilder.add_header(headerOffset); - imageBuilder.add_labels_bb(boxes2DLabeledOffset); - imageBuilder.add_labels_general(generalLabelsOffset); + imageBuilder.add_labels(labelsOffset); auto imageOffset = imageBuilder.Finish(); builder.Finish(imageOffset); diff --git a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_point.cpp b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_point.cpp index 584b74661..4fdf846f9 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_point.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_point.cpp @@ -43,7 +43,7 @@ void Hdf5FbPoint::writePoint(const std::string& id, const seerep::fb::PointStamp writeAttributeMap(data_set_ptr, point->attribute()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, point->labels_general()); + writeLabels(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, point->labels()); m_file->flush(); } @@ -99,13 +99,13 @@ std::optional> Hdf5FbPoint: auto headerOffset = readHeaderAttributes(builder, *data_set_ptr, id); - auto labelsGeneralOffset = readGeneralLabels(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, builder); + auto labelsOffset = readLabels(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, builder); seerep::fb::PointStampedBuilder pointStampedBuilder(builder); pointStampedBuilder.add_attribute(attributeMapOffset); pointStampedBuilder.add_header(headerOffset); - pointStampedBuilder.add_labels_general(labelsGeneralOffset); + pointStampedBuilder.add_labels(labelsOffset); pointStampedBuilder.add_point(pointOffset); auto pointStampedOffset = pointStampedBuilder.Finish(); 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 3ab729f54..b8876d298 100644 --- a/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_pointcloud.cpp @@ -25,8 +25,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: writePointFieldAttributes(*dataGroupPtr, pcl.fields()); - writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, pcl.labels_bb()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, pcl.labels_general()); + writeLabels(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, pcl.labels()); HighFive::DataSpace dataSpace{ pcl.data()->size() }; std::shared_ptr payloadPtr = getHdf5DataSet(hdf5GroupPath + "/points", dataSpace); @@ -84,16 +83,6 @@ 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) { @@ -161,11 +150,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) payloadDataset.read(data); } - auto labelsGeneralOffset = - readGeneralLabels(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, builder); - - auto boundingBoxLabeledOffset = - readBoundingBoxesLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, builder); + auto labelsOffset = readLabels(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, builder); // build the point cloud message seerep::fb::PointCloud2Builder pointCloudBuilder(builder); @@ -181,8 +166,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) pointCloudBuilder.add_data(dataOffset); } pointCloudBuilder.add_is_dense(isDense); - pointCloudBuilder.add_labels_general(labelsGeneralOffset); - pointCloudBuilder.add_labels_bb(boundingBoxLabeledOffset); + pointCloudBuilder.add_labels(labelsOffset); auto pointCloudOffset = pointCloudBuilder.Finish(); builder.Finish(pointCloudOffset); diff --git a/seerep_hdf5/seerep_hdf5_fb/test/fb_write_load_test.cpp b/seerep_hdf5/seerep_hdf5_fb/test/fb_write_load_test.cpp index 63102582c..41e8657a8 100644 --- a/seerep_hdf5/seerep_hdf5_fb/test/fb_write_load_test.cpp +++ b/seerep_hdf5/seerep_hdf5_fb/test/fb_write_load_test.cpp @@ -78,74 +78,39 @@ createImageData(flatbuffers::FlatBufferBuilder& fbb, const unsigned int imageHei return fbImageDataOffset; } -flatbuffers::Offset createLabelWithInstance(flatbuffers::FlatBufferBuilder& fbb) +flatbuffers::Offset createLabel(flatbuffers::FlatBufferBuilder& fbb) { boost::uuids::uuid instanceUUID = boost::uuids::random_generator()(); auto instanceUUIDOffset = fbb.CreateString(boost::lexical_cast(instanceUUID)); auto labelStr = fbb.CreateString("testLabelGeneral"); - auto label = seerep::fb::CreateLabel(fbb, labelStr, 0.5); - - auto labelWithInstanceOffset = seerep::fb::CreateLabelWithInstance(fbb, label, instanceUUIDOffset); - fbb.Finish(labelWithInstanceOffset); - return labelWithInstanceOffset; + auto labelOffset = seerep::fb::CreateLabel(fbb, labelStr, 42, instanceUUIDOffset, 43); + fbb.Finish(labelOffset); + return labelOffset; } -flatbuffers::Offset>> -createBB2DLabeled(flatbuffers::FlatBufferBuilder& fbb) +flatbuffers::Offset>> +createLabels(flatbuffers::FlatBufferBuilder& fbb) { - std::vector> bbCategory; + std::vector> labelsCategories; for (size_t iCategory = 0; iCategory < 3; iCategory++) { auto categoryOffset = fbb.CreateString("category" + std::to_string(iCategory)); - std::vector> bbLabeled; + std::vector> labelsWithInstanceVector; for (size_t i = 0; i < 10; i++) { - auto pointMinOffset = createPoint(fbb, 0.01 + i / 10, 0.02 + i / 10); - auto pointMaxOffset = createPoint(fbb, 0.03 + i / 10, 0.04 + i / 10); - - auto bb2DOffset = seerep::fb::CreateBoundingbox2D(fbb, pointMinOffset, pointMaxOffset); - fbb.Finish(bb2DOffset); - - auto labelWithInstanceOffset = createLabelWithInstance(fbb); - - seerep::fb::BoundingBox2DLabeledBuilder bbLabeledBuilder(fbb); - bbLabeledBuilder.add_bounding_box(bb2DOffset); - bbLabeledBuilder.add_labelWithInstance(labelWithInstanceOffset); - bbLabeled.push_back(bbLabeledBuilder.Finish()); + labelsWithInstanceVector.push_back(createLabel(fbb)); } - auto bb2dLabeledOffset = fbb.CreateVector(bbLabeled); - seerep::fb::BoundingBox2DLabeledWithCategoryBuilder bbCategoryBuilder(fbb); - bbCategoryBuilder.add_category(categoryOffset); - bbCategoryBuilder.add_boundingBox2dLabeled(bb2dLabeledOffset); - bbCategory.push_back(bbCategoryBuilder.Finish()); - } - auto labelsBBOffset = fbb.CreateVector(bbCategory); - fbb.Finish(labelsBBOffset); - return labelsBBOffset; -} - -flatbuffers::Offset>> -createLabelsGeneral(flatbuffers::FlatBufferBuilder& fbb) -{ - std::vector> labelsCategories; - for (size_t iCategory = 0; iCategory < 3; iCategory++) - { - auto categoryOffset = fbb.CreateString("category" + std::to_string(iCategory)); - - std::vector> labelsWithInstanceVector; - for (size_t i = 0; i < 10; i++) - { - labelsWithInstanceVector.push_back(createLabelWithInstance(fbb)); - } + auto labelOffset = fbb.CreateVector(labelsWithInstanceVector); - auto labelWithInstanceOffset = fbb.CreateVector(labelsWithInstanceVector); + auto datumaroJsonOffset = fbb.CreateString("random string to test datumaro field"); - seerep::fb::LabelsWithInstanceWithCategoryBuilder labelsCategoriesBuilder(fbb); + seerep::fb::LabelCategoryBuilder labelsCategoriesBuilder(fbb); labelsCategoriesBuilder.add_category(categoryOffset); - labelsCategoriesBuilder.add_labelsWithInstance(labelWithInstanceOffset); + labelsCategoriesBuilder.add_labels(labelOffset); + labelsCategoriesBuilder.add_datumaroJson(datumaroJsonOffset); labelsCategories.push_back(labelsCategoriesBuilder.Finish()); } auto labelsCategoriesOffset = fbb.CreateVector(labelsCategories); @@ -163,12 +128,11 @@ const seerep::fb::Image* createImageMessage(flatbuffers::FlatBufferBuilder& fbb, auto headerOffset = createHeader(fbb, "camera", projectUUID, messageUUID); auto imageOffset = createImageData(fbb, 256, 256); - auto generalLabelsOffset = createLabelsGeneral(fbb); - auto bB2DLabeledOffset = createBB2DLabeled(fbb); + auto generalLabelsOffset = createLabels(fbb); auto imgMsgOffset = seerep::fb::CreateImage(fbb, headerOffset, imageHeight, imageWidth, encodingOffset, true, 3 * imageHeight, - imageOffset, generalLabelsOffset, bB2DLabeledOffset, camintrinsicsUUIDOffset); + imageOffset, generalLabelsOffset, camintrinsicsUUIDOffset); fbb.Finish(imgMsgOffset); uint8_t* buf = fbb.GetBufferPointer(); return flatbuffers::GetRoot(buf); @@ -281,38 +245,38 @@ TEST_F(fbWriteLoadTest, testImageData) } } -void testLabelWithInstance(const seerep::fb::LabelWithInstance* readInstance, - const seerep::fb::LabelWithInstance* writeInstance) +void testLabel(const seerep::fb::Label* readInstance, const seerep::fb::Label* writeInstance) { if (readInstance == nullptr || writeInstance == nullptr) { - FAIL() << "Error: Can't compare a LabelWithInstance to nullptr"; + FAIL() << "Error: Can't compare a Label to nullptr"; } - EXPECT_STREQ(readInstance->label()->label()->c_str(), writeInstance->label()->label()->c_str()); - EXPECT_FLOAT_EQ(readInstance->label()->confidence(), writeInstance->label()->confidence()); + EXPECT_STREQ(readInstance->label()->c_str(), writeInstance->label()->c_str()); + EXPECT_EQ(readInstance->labelIdDatumaro(), writeInstance->labelIdDatumaro()); EXPECT_STREQ(readInstance->instanceUuid()->c_str(), writeInstance->instanceUuid()->c_str()); + EXPECT_EQ(readInstance->instanceIdDatumaro(), writeInstance->instanceIdDatumaro()); } -void testLabelWithInstanceCategories(const seerep::fb::LabelsWithInstanceWithCategory* readInstance, - const seerep::fb::LabelsWithInstanceWithCategory* writeInstance) +void testLabelCategories(const seerep::fb::LabelCategory* readInstance, const seerep::fb::LabelCategory* writeInstance) { if (readInstance == nullptr || writeInstance == nullptr) { - FAIL() << "Error: Can't compare a LabelWithInstance to nullptr"; + FAIL() << "Error: Can't compare a Label to nullptr"; } EXPECT_STREQ(readInstance->category()->c_str(), writeInstance->category()->c_str()); - for (size_t i = 0; i < readInstance->labelsWithInstance()->size(); i++) + for (size_t i = 0; i < readInstance->labels()->size(); i++) { - testLabelWithInstance(readInstance->labelsWithInstance()->Get(i), writeInstance->labelsWithInstance()->Get(i)); + testLabel(readInstance->labels()->Get(i), writeInstance->labels()->Get(i)); } + EXPECT_STREQ(readInstance->datumaroJson()->c_str(), writeInstance->datumaroJson()->c_str()); } TEST_F(fbWriteLoadTest, testGeneralLabels) { - ASSERT_EQ(readImage->labels_general()->size(), writeImage->labels_general()->size()); - for (size_t i = 0; i < readImage->labels_general()->size(); i++) + ASSERT_EQ(readImage->labels()->size(), writeImage->labels()->size()); + for (size_t i = 0; i < readImage->labels()->size(); i++) { - testLabelWithInstanceCategories(readImage->labels_general()->Get(i), writeImage->labels_general()->Get(i)); + testLabelCategories(readImage->labels()->Get(i), writeImage->labels()->Get(i)); } } @@ -326,48 +290,6 @@ void testEqualPoints(const seerep::fb::Point2D* readPoint, const seerep::fb::Poi EXPECT_EQ(readPoint->y(), writePoint->y()); } -void testBB2DWithInstance(const seerep::fb::BoundingBox2DLabeled* readInstance, - const seerep::fb::BoundingBox2DLabeled* writeInstance) -{ - if (readInstance == nullptr || writeInstance == nullptr) - { - FAIL() << "Error: Can't compare a LabelWithInstance to nullptr"; - } - EXPECT_STREQ(readInstance->labelWithInstance()->label()->label()->c_str(), - writeInstance->labelWithInstance()->label()->label()->c_str()); - EXPECT_FLOAT_EQ(readInstance->labelWithInstance()->label()->confidence(), - writeInstance->labelWithInstance()->label()->confidence()); - EXPECT_STREQ(readInstance->labelWithInstance()->instanceUuid()->c_str(), - writeInstance->labelWithInstance()->instanceUuid()->c_str()); - - testEqualPoints(readInstance->bounding_box()->center_point(), writeInstance->bounding_box()->center_point()); - testEqualPoints(readInstance->bounding_box()->spatial_extent(), writeInstance->bounding_box()->spatial_extent()); - EXPECT_FLOAT_EQ(readInstance->bounding_box()->rotation(), writeInstance->bounding_box()->rotation()); -} - -void testBB2DWithInstanceCategories(const seerep::fb::BoundingBox2DLabeledWithCategory* readInstance, - const seerep::fb::BoundingBox2DLabeledWithCategory* writeInstance) -{ - if (readInstance == nullptr || writeInstance == nullptr) - { - FAIL() << "Error: Can't compare a LabelWithInstance to nullptr"; - } - EXPECT_STREQ(readInstance->category()->c_str(), writeInstance->category()->c_str()); - for (size_t i = 0; i < readInstance->boundingBox2dLabeled()->size(); i++) - { - testBB2DWithInstance(readInstance->boundingBox2dLabeled()->Get(i), writeInstance->boundingBox2dLabeled()->Get(i)); - } -} - -TEST_F(fbWriteLoadTest, testBoundingBox2DLabeled) -{ - ASSERT_EQ(readImage->labels_bb()->size(), writeImage->labels_bb()->size()); - for (size_t i = 0; i < readImage->labels_bb()->size(); i++) - { - testBB2DWithInstanceCategories(readImage->labels_bb()->Get(i), writeImage->labels_bb()->Get(i)); - } -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/seerep_hdf5/seerep_hdf5_pb/include/seerep_hdf5_pb/hdf5_pb_general.h b/seerep_hdf5/seerep_hdf5_pb/include/seerep_hdf5_pb/hdf5_pb_general.h index aee698da7..65f2ef62a 100644 --- a/seerep_hdf5/seerep_hdf5_pb/include/seerep_hdf5_pb/hdf5_pb_general.h +++ b/seerep_hdf5/seerep_hdf5_pb/include/seerep_hdf5_pb/hdf5_pb_general.h @@ -8,10 +8,8 @@ #include // seerep-msgs -#include -#include #include -#include +#include // std #include @@ -39,28 +37,13 @@ class Hdf5PbGeneral : public virtual seerep_hdf5_core::Hdf5CoreGeneral seerep::pb::Header readHeaderAttributes(HighFive::AnnotateTraits& object, const std::string& id); // ################ - // BoundingBoxes + // Labels // ################ - void writeBoundingBoxLabeled( - const std::string& datatypeGroup, const std::string& uuid, - const google::protobuf::RepeatedPtrField<::seerep::pb::BoundingBoxLabeledWithCategory>& boundingboxLabeled); + void writeLabels(const std::string& datatypeGroup, const std::string& uuid, + const google::protobuf::RepeatedPtrField& labels); - void writeBoundingBox2DLabeled( - const std::string& datatypeGroup, const std::string& uuid, - const google::protobuf::RepeatedPtrField& boundingbox2DLabeled); - - std::optional> - readBoundingBox2DLabeled(const std::string& datatypeGroup, const std::string& uuid); - - // ################ - // Labels General - // ################ - void writeLabelsGeneral( - const std::string& datatypeGroup, const std::string& uuid, - const google::protobuf::RepeatedPtrField& labelsGeneralWithInstances); - - std::optional> - readLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid); + std::optional> + readLabels(const std::string& datatypeGroup, const std::string& uuid); }; } // namespace seerep_hdf5_pb diff --git a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_general.cpp b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_general.cpp index 7bc213c9f..66844c443 100644 --- a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_general.cpp +++ b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_general.cpp @@ -9,216 +9,57 @@ Hdf5PbGeneral::Hdf5PbGeneral(std::shared_ptr& file, std::shared_ { } -void Hdf5PbGeneral::writeBoundingBoxLabeled( - const std::string& datatypeGroup, const std::string& uuid, - const google::protobuf::RepeatedPtrField<::seerep::pb::BoundingBoxLabeledWithCategory>& - boundingboxLabeledWithCategory) +void Hdf5PbGeneral::writeLabels(const std::string& datatypeGroup, const std::string& uuid, + const google::protobuf::RepeatedPtrField& labelsCategoryPb) { - std::string id = datatypeGroup + "/" + uuid; - if (!boundingboxLabeledWithCategory.empty()) + if (!labelsCategoryPb.empty()) { - for (auto& boundingboxLabeled : boundingboxLabeledWithCategory) + std::vector labelCategoryVector; + for (auto& labelCategoryPb : labelsCategoryPb) { - if (!boundingboxLabeled.boundingboxlabeled().empty()) + std::vector labels, instances; + std::vector labelsIdDatumaro, instancesIdDatumaro; + for (auto& label : labelCategoryPb.labels()) { - std::vector labels; - std::vector labelConfidences; - std::vector> boundingBoxesWithRotation; - std::vector instances; - for (auto label : boundingboxLabeled.boundingboxlabeled()) - { - labels.push_back(label.labelwithinstance().label().label()); - labelConfidences.push_back(label.labelwithinstance().label().confidence()); - std::vector boxWithRotation{ - label.boundingbox().center_point().x(), label.boundingbox().center_point().y(), - label.boundingbox().center_point().z(), label.boundingbox().spatial_extent().x(), - label.boundingbox().spatial_extent().y(), label.boundingbox().spatial_extent().z(), - label.boundingbox().rotation().x(), label.boundingbox().rotation().y(), - label.boundingbox().rotation().z(), label.boundingbox().rotation().w() - }; - boundingBoxesWithRotation.push_back(boxWithRotation); - instances.push_back(label.labelwithinstance().instanceuuid()); - } - - HighFive::DataSet datasetLabels = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBB + "_" + boundingboxLabeled.category(), - HighFive::DataSpace::From(labels)); - datasetLabels.write(labels); - - HighFive::DataSet datasetLabelConfidences = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBCONFIDENCES + "_" + boundingboxLabeled.category(), - HighFive::DataSpace::From(labelConfidences)); - datasetLabelConfidences.write(labelConfidences); - - HighFive::DataSet datasetBoxesWithRotation = - m_file->createDataSet(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBBOXESWITHROTATION + "_" + - boundingboxLabeled.category(), - HighFive::DataSpace::From(boundingBoxesWithRotation)); - datasetBoxesWithRotation.write(boundingBoxesWithRotation); - - HighFive::DataSet datasetInstances = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBINSTANCES + "_" + boundingboxLabeled.category(), - HighFive::DataSpace::From(instances)); - datasetInstances.write(instances); - } - } - m_file->flush(); - } -} - -void Hdf5PbGeneral::writeBoundingBox2DLabeled( - const std::string& datatypeGroup, const std::string& uuid, - const google::protobuf::RepeatedPtrField<::seerep::pb::BoundingBox2DLabeledWithCategory>& - boundingbox2DLabeledWithCategory) -{ - std::string id = datatypeGroup + "/" + uuid; - if (!boundingbox2DLabeledWithCategory.empty()) - { - for (auto& boundingbox2DLabeled : boundingbox2DLabeledWithCategory) - { - if (!boundingbox2DLabeled.boundingbox2dlabeled().empty()) - { - std::vector labels; - std::vector labelConfidences; - std::vector> boundingBoxesWithRotation; - std::vector instances; - for (auto label : boundingbox2DLabeled.boundingbox2dlabeled()) - { - labels.push_back(label.labelwithinstance().label().label()); - labelConfidences.push_back(label.labelwithinstance().label().confidence()); - std::vector boxWithRotation{ label.boundingbox().center_point().x(), - label.boundingbox().center_point().y(), - label.boundingbox().spatial_extent().x(), - label.boundingbox().spatial_extent().y(), - label.boundingbox().rotation() }; - boundingBoxesWithRotation.push_back(boxWithRotation); - - instances.push_back(label.labelwithinstance().instanceuuid()); - } - - HighFive::DataSet datasetLabels = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBB + "_" + boundingbox2DLabeled.category(), - HighFive::DataSpace::From(labels)); - datasetLabels.write(labels); - - HighFive::DataSet datasetLabelConfidences = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBCONFIDENCES + "_" + boundingbox2DLabeled.category(), - HighFive::DataSpace::From(labelConfidences)); - datasetLabelConfidences.write(labelConfidences); - - HighFive::DataSet datasetBoxesWithRotation = - m_file->createDataSet(id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBBOXESWITHROTATION + "_" + - boundingbox2DLabeled.category(), - HighFive::DataSpace::From(boundingBoxesWithRotation)); - datasetBoxesWithRotation.write(boundingBoxesWithRotation); - - HighFive::DataSet datasetInstances = m_file->createDataSet( - id + "/" + seerep_hdf5_core::Hdf5CoreGeneral::LABELBBINSTANCES + "_" + boundingbox2DLabeled.category(), - HighFive::DataSpace::From(instances)); - datasetInstances.write(instances); - - m_file->flush(); + labels.push_back(label.label()); + labelsIdDatumaro.push_back(label.labeliddatumaro()); + instances.push_back(label.instanceuuid()); + instancesIdDatumaro.push_back(label.instanceiddatumaro()); } + seerep_core_msgs::LabelCategory labelCategory; + labelCategory.category = labelCategoryPb.category(); + labelCategory.labels = labels; + labelCategory.labelsIdDatumaro = labelsIdDatumaro; + labelCategory.instances = instances; + labelCategory.instancesIdDatumaro = instancesIdDatumaro; + labelCategoryVector.push_back(labelCategory); } + Hdf5CoreGeneral::writeLabels(datatypeGroup, uuid, labelCategoryVector); } } -std::optional> -Hdf5PbGeneral::readBoundingBox2DLabeled(const std::string& datatypeGroup, const std::string& uuid) +std::optional> +Hdf5PbGeneral::readLabels(const std::string& datatypeGroup, const std::string& uuid) { - std::vector labelCategories; - std::vector> labelsPerCategory; - std::vector> labelConfidencesPerCategory; - std::vector>> boundingBoxesPerCategory; - std::vector> instancesPerCategory; - - seerep_hdf5_core::Hdf5CoreGeneral::readBoundingBoxLabeled(datatypeGroup, uuid, labelCategories, labelsPerCategory, - labelConfidencesPerCategory, boundingBoxesPerCategory, - instancesPerCategory); + std::vector labelCategories, datumaroJsonPerCategory; + std::vector> labelsPerCategory; + seerep_hdf5_core::Hdf5CoreGeneral::readLabels(datatypeGroup, uuid, labelCategories, labelsPerCategory, + datumaroJsonPerCategory); - google::protobuf::RepeatedPtrField result; + google::protobuf::RepeatedPtrField result; for (size_t iCategory = 0; iCategory < labelCategories.size(); iCategory++) { - seerep::pb::BoundingBox2DLabeledWithCategory resultCat; + seerep::pb::LabelCategory resultCat; resultCat.set_category(labelCategories.at(iCategory)); + resultCat.set_datumarojson(datumaroJsonPerCategory.at(iCategory)); for (size_t i = 0; i < labelsPerCategory.at(iCategory).size(); i++) { - auto bblabeled = resultCat.add_boundingbox2dlabeled(); - bblabeled->mutable_labelwithinstance()->mutable_label()->set_label(labelsPerCategory.at(iCategory).at(i)); - bblabeled->mutable_labelwithinstance()->mutable_label()->set_confidence( - labelConfidencesPerCategory.at(iCategory).at(i)); - bblabeled->mutable_labelwithinstance()->set_instanceuuid(instancesPerCategory.at(iCategory).at(i)); - - bblabeled->mutable_boundingbox()->mutable_center_point()->set_x( - boundingBoxesPerCategory.at(iCategory).at(i).at(0)); - bblabeled->mutable_boundingbox()->mutable_center_point()->set_y( - boundingBoxesPerCategory.at(iCategory).at(i).at(1)); - bblabeled->mutable_boundingbox()->mutable_spatial_extent()->set_x( - boundingBoxesPerCategory.at(iCategory).at(i).at(2)); - bblabeled->mutable_boundingbox()->mutable_spatial_extent()->set_y( - boundingBoxesPerCategory.at(iCategory).at(i).at(3)); - bblabeled->mutable_boundingbox()->set_rotation(boundingBoxesPerCategory.at(iCategory).at(i).at(4)); - } - result.Add(std::move(resultCat)); - } - - return result; -} - -void Hdf5PbGeneral::writeLabelsGeneral( - const std::string& datatypeGroup, const std::string& uuid, - const google::protobuf::RepeatedPtrField& - labelsGeneralWithInstancesWithCategory) -{ - if (!labelsGeneralWithInstancesWithCategory.empty()) - { - std::vector labelsWithCategoryVector; - for (auto& labelsWithInstanceOfCategory : labelsGeneralWithInstancesWithCategory) - { - std::vector labels; - std::vector labelConfidences; - std::vector instances; - for (auto& labelWithInstances : labelsWithInstanceOfCategory.labelwithinstance()) - { - labels.push_back(labelWithInstances.label().label()); - labelConfidences.push_back(labelWithInstances.label().confidence()); - - instances.push_back(labelWithInstances.instanceuuid()); - } - seerep_core_msgs::LabelsWithInstanceWithCategory labelsWithCategory; - labelsWithCategory.category = labelsWithInstanceOfCategory.category(); - labelsWithCategory.labels = labels; - labelsWithCategory.labelConfidences = labelConfidences; - labelsWithCategory.instances = instances; - labelsWithCategoryVector.push_back(labelsWithCategory); - } - Hdf5CoreGeneral::writeLabelsGeneral(datatypeGroup, uuid, labelsWithCategoryVector); - } -} - -std::optional> -Hdf5PbGeneral::readLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid) -{ - std::vector labelCategoriesGeneral; - std::vector> labelsWithInstancesGeneralPerCategory; - seerep_hdf5_core::Hdf5CoreGeneral::readLabelsGeneral(datatypeGroup, uuid, labelCategoriesGeneral, - labelsWithInstancesGeneralPerCategory); - - google::protobuf::RepeatedPtrField result; - - for (size_t iCategory = 0; iCategory < labelCategoriesGeneral.size(); iCategory++) - { - seerep::pb::LabelsWithInstanceWithCategory resultCat; - resultCat.set_category(labelCategoriesGeneral.at(iCategory)); - for (size_t i = 0; i < labelsWithInstancesGeneralPerCategory.at(iCategory).size(); i++) - { - auto labelWithInstance = resultCat.add_labelwithinstance(); - labelWithInstance->mutable_label()->set_label(labelsWithInstancesGeneralPerCategory.at(iCategory).at(i).label); - labelWithInstance->mutable_label()->set_confidence( - labelsWithInstancesGeneralPerCategory.at(iCategory).at(i).labelConfidence); - labelWithInstance->set_instanceuuid( - boost::lexical_cast(labelsWithInstancesGeneralPerCategory.at(iCategory).at(i).uuidInstance)); + auto label = resultCat.add_labels(); + label->set_label(labelsPerCategory.at(iCategory).at(i).label); + label->set_labeliddatumaro(labelsPerCategory.at(iCategory).at(i).labelIdDatumaro); + label->set_instanceuuid(boost::lexical_cast(labelsPerCategory.at(iCategory).at(i).uuidInstance)); + label->set_instanceiddatumaro(labelsPerCategory.at(iCategory).at(i).instanceIdDatumaro); } result.Add(std::move(resultCat)); } diff --git a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_image.cpp b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_image.cpp index 2254a9468..ad31938d3 100644 --- a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_image.cpp +++ b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_image.cpp @@ -31,9 +31,7 @@ void Hdf5PbImage::writeImage(const std::string& id, const seerep::pb::Image& ima const uint8_t* arrayStartPtr = reinterpret_cast(image.data().c_str()); dataSetPtr->write(std::vector(arrayStartPtr, arrayStartPtr + image.data().size())); - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_bb()); - seerep_hdf5_pb::Hdf5PbGeneral::writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, - image.labels_general()); + seerep_hdf5_pb::Hdf5PbGeneral::writeLabels(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels()); m_file->flush(); } @@ -63,8 +61,7 @@ std::optional Hdf5PbImage::readImage(const std::string& id) auto ImageAttributes = readImageAttributes(id); - auto labelsBB = readBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id); - auto labelsGeneral = readLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id); + auto labels = readLabels(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id); std::vector data; dataSetPtr->read(data); @@ -83,13 +80,9 @@ std::optional Hdf5PbImage::readImage(const std::string& id) *image.mutable_data() = { data.begin(), data.end() }; - if (labelsBB) + if (labels) { - *image.mutable_labels_bb() = labelsBB.value(); - } - if (labelsGeneral) - { - *image.mutable_labels_general() = labelsGeneral.value(); + *image.mutable_labels() = labels.value(); } return image; } diff --git a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp index 670930161..8155ce9a7 100644 --- a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp +++ b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp @@ -58,8 +58,7 @@ std::shared_ptr Hdf5PbPointCloud::writePointCloud2(const std::s writePointFieldAttributes(*data_group_ptr, pointcloud2.fields()); writeHeaderAttributes(*data_group_ptr, pointcloud2.header()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels_general()); - writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels_bb()); + writeLabels(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels()); CloudInfo info = getCloudInfo(pointcloud2); diff --git a/seerep_hdf5/seerep_hdf5_pb/test/pb_write_load_test.cpp b/seerep_hdf5/seerep_hdf5_pb/test/pb_write_load_test.cpp index 16c411810..dbece5c06 100644 --- a/seerep_hdf5/seerep_hdf5_pb/test/pb_write_load_test.cpp +++ b/seerep_hdf5/seerep_hdf5_pb/test/pb_write_load_test.cpp @@ -30,23 +30,11 @@ void createHeader(const std::string projectUUID, const std::string messageUUID, header.set_seq(5); header.set_frame_id("arbitrary_id"); header.mutable_stamp()->set_seconds(5); - header.mutable_stamp()->set_nanos(5); + header.mutable_stamp()->set_nanos(6); header.set_uuid_project(projectUUID); header.set_uuid_msgs(messageUUID); } -/** - * @brief sets values in a pointer to a point, given x and y co ordinate values - * @param[in] x x co ordinate - * @param[out] y y co ordinate - * @param[in,out] point2D pointer to a 2D point object - * */ -void createPoint(const double x, const double y, seerep::pb::Point2D& point2D) -{ - point2D.set_x(x); - point2D.set_y(y); -} - /** * @brief creates a grid of image data given height and width of an image * @param[in] imageHeight the height of the image @@ -79,37 +67,16 @@ void createImageData(const unsigned int imageHeight, const unsigned int imageWid } /** - * @brief given a labelWithInstance set arbitrary label and uuid - * @param[in,out] labelWithInstance a pointer to a label with instance + * @brief given a label set arbitrary label and uuid + * @param[in,out] label a pointer to a label with instance * */ -void createLabelWithInstance(seerep::pb::LabelWithInstance& labelWithInstance) +void createLabel(seerep::pb::Label& label) { boost::uuids::uuid instanceUUID = boost::uuids::random_generator()(); - labelWithInstance.mutable_label()->set_label("arbitrary_instance_label"); - labelWithInstance.mutable_label()->set_confidence(0.5); - labelWithInstance.set_instanceuuid(boost::lexical_cast(instanceUUID)); -} - -/** - * @brief create a 2D Labeled Bounding Box - * @param[in] image reference to the image - * */ -void createBB2DLabeled(seerep::pb::Image& image) -{ - for (size_t iCategory = 0; iCategory < 3; iCategory++) - { - auto boundingBox2DLabeledWithCategory = image.add_labels_bb(); - boundingBox2DLabeledWithCategory->set_category("category" + std::to_string(iCategory)); - for (size_t i = 0; i < 10; i++) - { - auto bbLabeled = boundingBox2DLabeledWithCategory->add_boundingbox2dlabeled(); - createPoint(0.01 + i / 10, 0.02 + i / 10, *bbLabeled->mutable_boundingbox()->mutable_center_point()); - createPoint(0.03 + i / 10, 0.04 + i / 10, *bbLabeled->mutable_boundingbox()->mutable_spatial_extent()); - bbLabeled->mutable_boundingbox()->set_rotation(1.2); - - createLabelWithInstance(*bbLabeled->mutable_labelwithinstance()); - } - } + label.set_label("arbitrary_instance_label"); + label.set_labeliddatumaro(42); + label.set_instanceuuid(boost::lexical_cast(instanceUUID)); + label.set_instanceiddatumaro(43); } /** @@ -120,11 +87,12 @@ void createLabelsGeneral(seerep::pb::Image& image) { for (size_t iCategory = 0; iCategory < 3; iCategory++) { - auto labelsGeneral = image.add_labels_general(); - labelsGeneral->set_category("category" + std::to_string(iCategory)); + auto lables = image.add_labels(); + lables->set_category("category" + std::to_string(iCategory)); + lables->set_datumarojson("random string to test datumaro field"); for (size_t i = 0; i < 10; i++) { - createLabelWithInstance(*labelsGeneral->add_labelwithinstance()); + createLabel(*lables->add_labels()); } } } @@ -157,7 +125,6 @@ seerep::pb::Image createImageMessage(const unsigned int imageHeight, const unsig imgMsg.set_uuid_camera_intrinsics(cameraintrinsicsUUID); createLabelsGeneral(imgMsg); - createBB2DLabeled(imgMsg); return imgMsg; } @@ -288,24 +255,24 @@ TEST_F(pbWriteLoadTest, testImageData) /** * @brief test original label with instance and converted label with instance read from hdf5 file for equality. - * @param[in] readInstance the labelWithInstance which was read - * @param[in] writeInstance the labelWithInstance which was written + * @param[in] readInstance the Label which was read + * @param[in] writeInstance the Label which was written * */ -void testLabelWithInstance(const seerep::pb::LabelWithInstance& readInstance, - const seerep::pb::LabelWithInstance& writeInstance) +void testLabelWithInstance(const seerep::pb::Label& readInstance, const seerep::pb::Label& writeInstance) { - EXPECT_STREQ(readInstance.label().label().c_str(), writeInstance.label().label().c_str()); - EXPECT_FLOAT_EQ(readInstance.label().confidence(), writeInstance.label().confidence()); + EXPECT_STREQ(readInstance.label().c_str(), writeInstance.label().c_str()); + EXPECT_EQ(readInstance.labeliddatumaro(), writeInstance.labeliddatumaro()); EXPECT_STREQ(readInstance.instanceuuid().c_str(), writeInstance.instanceuuid().c_str()); + EXPECT_EQ(readInstance.instanceiddatumaro(), writeInstance.instanceiddatumaro()); } -void testLabelsWithInstanceWithCategory(const seerep::pb::LabelsWithInstanceWithCategory& readInstance, - const seerep::pb::LabelsWithInstanceWithCategory& writeInstance) +void testLabelsWithInstanceWithCategory(const seerep::pb::LabelCategory& readInstance, + const seerep::pb::LabelCategory& writeInstance) { EXPECT_STREQ(readInstance.category().c_str(), writeInstance.category().c_str()); - for (int i = 0; i < readInstance.labelwithinstance_size(); i++) + for (int i = 0; i < readInstance.labels_size(); i++) { - testLabelWithInstance(readInstance.labelwithinstance().Get(i), writeInstance.labelwithinstance().Get(i)); + testLabelWithInstance(readInstance.labels().Get(i), writeInstance.labels().Get(i)); } } @@ -316,47 +283,10 @@ void testLabelsWithInstanceWithCategory(const seerep::pb::LabelsWithInstanceWith * */ TEST_F(pbWriteLoadTest, testGeneralLabels) { - ASSERT_EQ(readImage.labels_general().size(), writeImage.labels_general().size()); - for (int i = 0; i < readImage.labels_general().size(); i++) + ASSERT_EQ(readImage.labels().size(), writeImage.labels().size()); + for (int i = 0; i < readImage.labels().size(); i++) { - testLabelsWithInstanceWithCategory(readImage.labels_general().Get(i), writeImage.labels_general().Get(i)); - } -} - -/** - * @brief test original point and converted point read from hdf5 file for equality. - * @param[in] readPoint the Point2D which was read - * @param[in] writePoint the Point2D which was written - * */ -void testEqualPoints(const seerep::pb::Point2D& readPoint, const seerep::pb::Point2D& writePoint) -{ - EXPECT_EQ(readPoint.x(), writePoint.x()); - EXPECT_EQ(readPoint.y(), writePoint.y()); -} - -/** - * @brief test original bounding box 2d labeled and converted bounding box 2d labeled read from hdf5 file for equality. - * @param[in] pbWriteLoadTest test suite class - * @param testImageHeader name of test - * */ -TEST_F(pbWriteLoadTest, testBoundingBox2DLabeled) -{ - for (int iCategory = 0; iCategory < readImage.labels_bb_size(); iCategory++) - { - ASSERT_EQ(readImage.labels_bb().size(), writeImage.labels_bb().size()); - EXPECT_STREQ(readImage.labels_bb().at(iCategory).category().c_str(), - writeImage.labels_bb().at(iCategory).category().c_str()); - for (int i = 0; i < readImage.labels_bb().at(iCategory).boundingbox2dlabeled_size(); i++) - { - testLabelWithInstance(readImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).labelwithinstance(), - writeImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).labelwithinstance()); - testEqualPoints(readImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).boundingbox().center_point(), - writeImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).boundingbox().center_point()); - testEqualPoints(readImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).boundingbox().spatial_extent(), - writeImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).boundingbox().spatial_extent()); - EXPECT_FLOAT_EQ(readImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).boundingbox().rotation(), - writeImage.labels_bb().at(iCategory).boundingbox2dlabeled().Get(i).boundingbox().rotation()); - } + testLabelsWithInstanceWithCategory(readImage.labels().Get(i), writeImage.labels().Get(i)); } } diff --git a/seerep_hdf5/seerep_hdf5_py/CATKIN_IGNORE b/seerep_hdf5/seerep_hdf5_py/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 1e26c84a4..b12b3eecc 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -15,123 +15,79 @@ set(Boost_USE_STATIC_RUNTIME OFF) find_package(Boost REQUIRED) set(MY_PROTO_FILES - protos/boundingbox_labeled.proto - protos/boundingbox_labeled_with_category.proto - protos/boundingbox_stamped.proto protos/boundingbox.proto - protos/boundingbox2d_labeled.proto - protos/boundingbox2d_labeled_with_category.proto - protos/boundingbox2d_stamped.proto - protos/boundingbox2d.proto protos/camera_intrinsics.proto protos/camera_intrinsics_query.proto - protos/categories.proto protos/datatype.proto - protos/frame_infos.proto protos/frame_query.proto protos/geodetic_coordinates.proto protos/header.proto protos/image.proto - protos/label_with_instance.proto + protos/label_category.proto protos/label.proto - protos/labels.proto - protos/labels_with_category.proto - protos/labels_with_instance_with_category.proto protos/point_cloud_2.proto protos/point_field.proto protos/point_stamped.proto protos/point.proto protos/point2d.proto - protos/polygon_stamped.proto - protos/polygon.proto protos/polygon2d.proto - protos/pose_stamped.proto - protos/pose.proto protos/project_info.proto protos/project_infos.proto protos/projectCreation.proto - protos/quaternion_stamped.proto protos/quaternion.proto protos/query.proto protos/region_of_interest.proto protos/server_response.proto + protos/string_vector.proto protos/time_interval.proto protos/timestamp.proto protos/transform_stamped.proto protos/transform_stamped_query.proto protos/transform.proto - protos/twist_stamped.proto - protos/twist_with_covariance_stamped.proto - protos/twist_with_covariance.proto - protos/twist.proto protos/uuid_datatype_with_category.proto protos/uuid_datatype_pair.proto - protos/vector3_stamped.proto protos/vector3.proto ) set(MY_FBS_FILES fbs/attributes_stamped.fbs - fbs/boundingbox_labeled.fbs - fbs/boundingbox_labeled_with_category.fbs - fbs/boundingbox_stamped.fbs fbs/boundingbox.fbs - fbs/boundingbox2d_labeled.fbs - 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 - fbs/categories.fbs fbs/datatype.fbs fbs/empty.fbs - fbs/frame_infos.fbs fbs/frame_query.fbs fbs/geodeticCoordinates.fbs fbs/header.fbs fbs/image.fbs - fbs/label_with_instance.fbs + fbs/label_category.fbs fbs/label.fbs - fbs/labels.fbs - fbs/labels_with_category.fbs - fbs/labels_with_instance_with_category.fbs fbs/point_cloud_2.fbs fbs/point_field.fbs fbs/point_stamped.fbs fbs/point.fbs fbs/point2d.fbs - fbs/polygon_stamped.fbs - fbs/polygon.fbs fbs/polygon2d.fbs - fbs/pose_stamped.fbs - fbs/pose.fbs fbs/project_info.fbs fbs/project_infos.fbs fbs/projectCreation.fbs - fbs/quaternion_stamped.fbs fbs/quaternion.fbs fbs/query.fbs fbs/query_instance.fbs fbs/region_of_interest.fbs fbs/server_response.fbs fbs/sparql_query.fbs + fbs/string_vector.fbs fbs/time_interval.fbs fbs/timestamp.fbs fbs/transform_stamped.fbs fbs/transform_stamped_query.fbs fbs/transform.fbs - fbs/twist_stamped.fbs - fbs/twist_with_covariance_stamped.fbs - fbs/twist_with_covariance.fbs - fbs/twist.fbs fbs/union_map_entry.fbs fbs/uuids_per_project.fbs fbs/uuids_project_pair.fbs fbs/uuid_datatype_with_category.fbs fbs/uuid_datatype_pair.fbs - fbs/vector3_stamped.fbs fbs/vector3.fbs ) @@ -142,7 +98,8 @@ set(MY_CORE_MSGS ${CMAKE_CURRENT_SOURCE_DIR}/core/camera_intrinsics_query.h ${CMAKE_CURRENT_SOURCE_DIR}/core/header.h ${CMAKE_CURRENT_SOURCE_DIR}/core/datatype.h - ${CMAKE_CURRENT_SOURCE_DIR}/core/label_with_instance.h + ${CMAKE_CURRENT_SOURCE_DIR}/core/label.h + ${CMAKE_CURRENT_SOURCE_DIR}/core/label_category.h ${CMAKE_CURRENT_SOURCE_DIR}/core/project_info.h ${CMAKE_CURRENT_SOURCE_DIR}/core/polygon2d.h ${CMAKE_CURRENT_SOURCE_DIR}/core/query_result_project.h diff --git a/seerep_msgs/core/dataset_indexable.h b/seerep_msgs/core/dataset_indexable.h index e13de56a6..b4c8bb59e 100644 --- a/seerep_msgs/core/dataset_indexable.h +++ b/seerep_msgs/core/dataset_indexable.h @@ -5,7 +5,7 @@ #include "aabb.h" #include "header.h" -#include "label_with_instance.h" +#include "label.h" namespace seerep_core_msgs { @@ -13,7 +13,7 @@ struct DatasetIndexable { Header header; AABB boundingbox; - std::unordered_map> labelsWithInstancesWithCategory; + std::unordered_map> labelsCategory; }; } /* namespace seerep_core_msgs */ diff --git a/seerep_msgs/core/label.h b/seerep_msgs/core/label.h new file mode 100644 index 000000000..c8700c0e7 --- /dev/null +++ b/seerep_msgs/core/label.h @@ -0,0 +1,19 @@ +#ifndef SEEREP_CORE_MSGS_LABEL_H_ +#define SEEREP_CORE_MSGS_LABEL_H_ + +#include +#include + +namespace seerep_core_msgs +{ +struct Label +{ + std::string label; + int labelIdDatumaro; + boost::uuids::uuid uuidInstance; + int instanceIdDatumaro; +}; + +} /* namespace seerep_core_msgs */ + +#endif // SEEREP_CORE_MSGS_LABEL_H_ diff --git a/seerep_msgs/core/label_category.h b/seerep_msgs/core/label_category.h new file mode 100644 index 000000000..b2cfa5d0f --- /dev/null +++ b/seerep_msgs/core/label_category.h @@ -0,0 +1,20 @@ +#ifndef SEEREP_CORE_MSGS_LABEL_CATEGORY_H_ +#define SEEREP_CORE_MSGS_LABEL_CATEGORY_H_ + +#include + +namespace seerep_core_msgs +{ +struct LabelCategory +{ + std::string category; + std::vector labels; + std::vector labelsIdDatumaro; + std::vector instances; + std::vector instancesIdDatumaro; + std::string datumaroJson; +}; + +} /* namespace seerep_core_msgs */ + +#endif // SEEREP_CORE_MSGS_LABEL_CATEGORY_H_ diff --git a/seerep_msgs/core/label_with_instance.h b/seerep_msgs/core/label_with_instance.h deleted file mode 100644 index 905d7142c..000000000 --- a/seerep_msgs/core/label_with_instance.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef SEEREP_CORE_MSGS_LABEL_WITH_INSTANCE_H_ -#define SEEREP_CORE_MSGS_LABEL_WITH_INSTANCE_H_ - -#include -#include - -namespace seerep_core_msgs -{ -struct LabelWithInstance -{ - std::string label; - float labelConfidence; - boost::uuids::uuid uuidInstance; -}; - -} /* namespace seerep_core_msgs */ - -#endif // SEEREP_CORE_MSGS_LABEL_WITH_INSTANCE_H_ diff --git a/seerep_msgs/core/labels_with_instance_with_category.h b/seerep_msgs/core/labels_with_instance_with_category.h deleted file mode 100644 index 720276379..000000000 --- a/seerep_msgs/core/labels_with_instance_with_category.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef SEEREP_CORE_MSGS_LABELS_WITH_INSTANCE_WITH_CATEGORY_H_ -#define SEEREP_CORE_MSGS_LABELS_WITH_INSTANCE_WITH_CATEGORY_H_ - -#include - -#include "label_with_instance.h" - -namespace seerep_core_msgs -{ -struct LabelsWithInstanceWithCategory -{ - std::string category; - std::vector labels; - std::vector labelConfidences; - std::vector instances; -}; - -} /* namespace seerep_core_msgs */ - -#endif // SEEREP_CORE_MSGS_LABELS_WITH_INSTANCE_WITH_CATEGORY_H_ diff --git a/seerep_msgs/fbs/boundingbox2d.fbs b/seerep_msgs/fbs/boundingbox2d.fbs deleted file mode 100644 index c32f47ce5..000000000 --- a/seerep_msgs/fbs/boundingbox2d.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "point2d.fbs"; - -namespace seerep.fb; - -table Boundingbox2D { - center_point:Point2D; - spatial_extent:Point2D; - rotation:float; -} diff --git a/seerep_msgs/fbs/boundingbox2d_labeled.fbs b/seerep_msgs/fbs/boundingbox2d_labeled.fbs deleted file mode 100644 index e2539486e..000000000 --- a/seerep_msgs/fbs/boundingbox2d_labeled.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "boundingbox2d.fbs"; -include "label_with_instance.fbs"; - -namespace seerep.fb; - -table BoundingBox2DLabeled { - labelWithInstance:LabelWithInstance; - bounding_box:seerep.fb.Boundingbox2D; -} diff --git a/seerep_msgs/fbs/boundingbox2d_labeled_with_category.fbs b/seerep_msgs/fbs/boundingbox2d_labeled_with_category.fbs deleted file mode 100644 index 93e230e6f..000000000 --- a/seerep_msgs/fbs/boundingbox2d_labeled_with_category.fbs +++ /dev/null @@ -1,9 +0,0 @@ - -include "boundingbox2d_labeled.fbs"; - -namespace seerep.fb; - -table BoundingBox2DLabeledWithCategory { - category:string; - boundingBox2dLabeled:[BoundingBox2DLabeled]; -} diff --git a/seerep_msgs/fbs/boundingbox2d_stamped.fbs b/seerep_msgs/fbs/boundingbox2d_stamped.fbs deleted file mode 100644 index 57715bbd3..000000000 --- a/seerep_msgs/fbs/boundingbox2d_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "boundingbox2d.fbs"; - -namespace seerep.fb; - -table Boundingbox2DStamped { - header:seerep.fb.Header; - boundingbox2D:seerep.fb.Boundingbox2D; -} diff --git a/seerep_msgs/fbs/boundingbox_labeled.fbs b/seerep_msgs/fbs/boundingbox_labeled.fbs deleted file mode 100644 index f884a2a7b..000000000 --- a/seerep_msgs/fbs/boundingbox_labeled.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "boundingbox.fbs"; -include "label_with_instance.fbs"; - -namespace seerep.fb; - -table BoundingBoxLabeled { - labelWithInstance:LabelWithInstance; - bounding_box:seerep.fb.Boundingbox; -} diff --git a/seerep_msgs/fbs/boundingbox_labeled_with_category.fbs b/seerep_msgs/fbs/boundingbox_labeled_with_category.fbs deleted file mode 100644 index e494cd25c..000000000 --- a/seerep_msgs/fbs/boundingbox_labeled_with_category.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "boundingbox_labeled.fbs"; -include "label_with_instance.fbs"; - -namespace seerep.fb; - -table BoundingBoxLabeledWithCategory { - category:string; - boundingBoxLabeled:[BoundingBoxLabeled]; -} diff --git a/seerep_msgs/fbs/boundingbox_stamped.fbs b/seerep_msgs/fbs/boundingbox_stamped.fbs deleted file mode 100644 index 10d064671..000000000 --- a/seerep_msgs/fbs/boundingbox_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "boundingbox.fbs"; - -namespace seerep.fb; - -table BoundingboxStamped { - header:seerep.fb.Header; - boundingbox:seerep.fb.Boundingbox; -} diff --git a/seerep_msgs/fbs/boundingboxes2d_labeled_stamped.fbs b/seerep_msgs/fbs/boundingboxes2d_labeled_stamped.fbs deleted file mode 100644 index 2e8609f15..000000000 --- a/seerep_msgs/fbs/boundingboxes2d_labeled_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "boundingbox2d_labeled_with_category.fbs"; - -namespace seerep.fb; - -table BoundingBoxes2DLabeledStamped { - header:seerep.fb.Header; - labels_bb:[seerep.fb.BoundingBox2DLabeledWithCategory]; -} diff --git a/seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs b/seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs deleted file mode 100644 index 98599515e..000000000 --- a/seerep_msgs/fbs/boundingboxes_labeled_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -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_msgs/fbs/categories.fbs b/seerep_msgs/fbs/categories.fbs deleted file mode 100644 index 917ae8866..000000000 --- a/seerep_msgs/fbs/categories.fbs +++ /dev/null @@ -1,6 +0,0 @@ - -namespace seerep.fb; - -table Categories { - categories:[string]; -} diff --git a/seerep_msgs/fbs/frame_infos.fbs b/seerep_msgs/fbs/frame_infos.fbs deleted file mode 100644 index 67872ff99..000000000 --- a/seerep_msgs/fbs/frame_infos.fbs +++ /dev/null @@ -1,6 +0,0 @@ -namespace seerep.fb; - -table FrameInfos -{ - frames:[string]; -} diff --git a/seerep_msgs/fbs/image.fbs b/seerep_msgs/fbs/image.fbs index e9b26c1f3..eaeae67ee 100644 --- a/seerep_msgs/fbs/image.fbs +++ b/seerep_msgs/fbs/image.fbs @@ -1,7 +1,6 @@ include "header.fbs"; -include "boundingbox2d_labeled_with_category.fbs"; -include "labels_with_instance_with_category.fbs"; +include "label_category.fbs"; namespace seerep.fb; @@ -13,7 +12,6 @@ table Image { is_bigendian:bool; step:uint; data:[ubyte]; - labels_general:[LabelsWithInstanceWithCategory]; - labels_bb:[BoundingBox2DLabeledWithCategory]; + labels:[LabelCategory]; uuid_cameraintrinsics:string; } diff --git a/seerep_msgs/fbs/label.fbs b/seerep_msgs/fbs/label.fbs index 339685005..576338842 100644 --- a/seerep_msgs/fbs/label.fbs +++ b/seerep_msgs/fbs/label.fbs @@ -3,5 +3,7 @@ namespace seerep.fb; table Label { label:string; - confidence:float; + labelIdDatumaro:int; + instanceUuid:string; + instanceIdDatumaro:int; } diff --git a/seerep_msgs/fbs/labels_with_category.fbs b/seerep_msgs/fbs/label_category.fbs similarity index 65% rename from seerep_msgs/fbs/labels_with_category.fbs rename to seerep_msgs/fbs/label_category.fbs index 5e75623df..c3b1381ab 100644 --- a/seerep_msgs/fbs/labels_with_category.fbs +++ b/seerep_msgs/fbs/label_category.fbs @@ -3,7 +3,8 @@ include "label.fbs"; namespace seerep.fb; -table LabelsWithCategory { +table LabelCategory { category:string; labels:[Label]; + datumaroJson:string; } diff --git a/seerep_msgs/fbs/label_with_instance.fbs b/seerep_msgs/fbs/label_with_instance.fbs deleted file mode 100644 index 930926c84..000000000 --- a/seerep_msgs/fbs/label_with_instance.fbs +++ /dev/null @@ -1,9 +0,0 @@ - -include "label.fbs"; - -namespace seerep.fb; - -table LabelWithInstance { - label:Label; - instanceUuid:string; -} diff --git a/seerep_msgs/fbs/labels.fbs b/seerep_msgs/fbs/labels.fbs deleted file mode 100644 index 0bda02c3e..000000000 --- a/seerep_msgs/fbs/labels.fbs +++ /dev/null @@ -1,6 +0,0 @@ - -namespace seerep.fb; - -table Labels { - labels:[string]; -} diff --git a/seerep_msgs/fbs/labels_with_instance_with_category.fbs b/seerep_msgs/fbs/labels_with_instance_with_category.fbs deleted file mode 100644 index 51af5fd0e..000000000 --- a/seerep_msgs/fbs/labels_with_instance_with_category.fbs +++ /dev/null @@ -1,9 +0,0 @@ - -include "label_with_instance.fbs"; - -namespace seerep.fb; - -table LabelsWithInstanceWithCategory { - category:string; - labelsWithInstance:[LabelWithInstance]; -} diff --git a/seerep_msgs/fbs/point_cloud_2.fbs b/seerep_msgs/fbs/point_cloud_2.fbs index e90624ae7..f00c1e654 100644 --- a/seerep_msgs/fbs/point_cloud_2.fbs +++ b/seerep_msgs/fbs/point_cloud_2.fbs @@ -1,7 +1,6 @@ include "header.fbs"; -include "boundingbox_labeled_with_category.fbs"; -include "labels_with_instance_with_category.fbs"; +include "label_category.fbs"; include "point_field.fbs"; namespace seerep.fb; @@ -16,6 +15,5 @@ table PointCloud2 { row_step:uint; data:[ubyte]; is_dense:bool; - labels_general:[LabelsWithInstanceWithCategory]; - labels_bb:[BoundingBoxLabeledWithCategory]; + labels:[LabelCategory]; } diff --git a/seerep_msgs/fbs/point_stamped.fbs b/seerep_msgs/fbs/point_stamped.fbs index 7dc0e906a..9d1a577b0 100644 --- a/seerep_msgs/fbs/point_stamped.fbs +++ b/seerep_msgs/fbs/point_stamped.fbs @@ -1,7 +1,7 @@ include "header.fbs"; include "point.fbs"; -include "labels_with_instance_with_category.fbs"; +include "label_category.fbs"; include "union_map_entry.fbs"; namespace seerep.fb; @@ -9,6 +9,6 @@ namespace seerep.fb; table PointStamped { header:seerep.fb.Header; point:seerep.fb.Point; - labels_general:[LabelsWithInstanceWithCategory]; + labels:[LabelCategory]; attribute:[UnionMapEntry]; } diff --git a/seerep_msgs/fbs/polygon.fbs b/seerep_msgs/fbs/polygon.fbs deleted file mode 100644 index 5c1bb73d8..000000000 --- a/seerep_msgs/fbs/polygon.fbs +++ /dev/null @@ -1,8 +0,0 @@ - -include "point.fbs"; - -namespace seerep.fb; - -table Polygon { - points:[seerep.fb.Point]; -} diff --git a/seerep_msgs/fbs/polygon_stamped.fbs b/seerep_msgs/fbs/polygon_stamped.fbs deleted file mode 100644 index 2e5aa0815..000000000 --- a/seerep_msgs/fbs/polygon_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "polygon.fbs"; - -namespace seerep.fb; - -table PolygonStamped { - header:seerep.fb.Header; - polygon:seerep.fb.Polygon; -} diff --git a/seerep_msgs/fbs/pose.fbs b/seerep_msgs/fbs/pose.fbs deleted file mode 100644 index 245d64f68..000000000 --- a/seerep_msgs/fbs/pose.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "point.fbs"; -include "quaternion.fbs"; - -namespace seerep.fb; - -table Pose { - position:seerep.fb.Point; - orientation:seerep.fb.Quaternion; -} diff --git a/seerep_msgs/fbs/pose_stamped.fbs b/seerep_msgs/fbs/pose_stamped.fbs deleted file mode 100644 index 7309db629..000000000 --- a/seerep_msgs/fbs/pose_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "pose.fbs"; - -namespace seerep.fb; - -table PoseStamped { - header:seerep.fb.Header; - pose:seerep.fb.Pose; -} diff --git a/seerep_msgs/fbs/quaternion_stamped.fbs b/seerep_msgs/fbs/quaternion_stamped.fbs deleted file mode 100644 index 260abc2a0..000000000 --- a/seerep_msgs/fbs/quaternion_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "quaternion.fbs"; - -namespace seerep.fb; - -table QuaternionStamped { - header:seerep.fb.Header; - quaternion:seerep.fb.Quaternion; -} diff --git a/seerep_msgs/fbs/query.fbs b/seerep_msgs/fbs/query.fbs index f32f08530..6ce910138 100644 --- a/seerep_msgs/fbs/query.fbs +++ b/seerep_msgs/fbs/query.fbs @@ -1,6 +1,6 @@ include "time_interval.fbs"; -include "labels_with_category.fbs"; +include "label_category.fbs"; include "polygon2d.fbs"; include "sparql_query.fbs"; @@ -11,7 +11,7 @@ table Query { fullyEncapsulated:bool; inMapFrame:bool; timeinterval:TimeInterval; - label:[LabelsWithCategory]; + label:[LabelCategory]; sparqlQuery:SparqlQuery; ontologyURI:string; mustHaveAllLabels:bool; diff --git a/seerep_msgs/fbs/string_vector.fbs b/seerep_msgs/fbs/string_vector.fbs new file mode 100644 index 000000000..1a89ac742 --- /dev/null +++ b/seerep_msgs/fbs/string_vector.fbs @@ -0,0 +1,6 @@ + +namespace seerep.fb; + +table StringVector { + stringVector:[string]; +} diff --git a/seerep_msgs/fbs/twist.fbs b/seerep_msgs/fbs/twist.fbs deleted file mode 100644 index 307e475dc..000000000 --- a/seerep_msgs/fbs/twist.fbs +++ /dev/null @@ -1,9 +0,0 @@ - -include "vector3.fbs"; - -namespace seerep.fb; - -table Twist { - linear:seerep.fb.Vector3; - angular:seerep.fb.Vector3; -} diff --git a/seerep_msgs/fbs/twist_stamped.fbs b/seerep_msgs/fbs/twist_stamped.fbs deleted file mode 100644 index 2163ec32c..000000000 --- a/seerep_msgs/fbs/twist_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "twist.fbs"; - -namespace seerep.fb; - -table TwistStamped { - header:seerep.fb.Header; - twist:seerep.fb.Twist; -} diff --git a/seerep_msgs/fbs/twist_with_covariance.fbs b/seerep_msgs/fbs/twist_with_covariance.fbs deleted file mode 100644 index 3cb817dc1..000000000 --- a/seerep_msgs/fbs/twist_with_covariance.fbs +++ /dev/null @@ -1,9 +0,0 @@ - -include "twist.fbs"; - -namespace seerep.fb; - -table TwistWithCovariance { - twist:seerep.fb.Twist; - covariance:[double]; -} diff --git a/seerep_msgs/fbs/twist_with_covariance_stamped.fbs b/seerep_msgs/fbs/twist_with_covariance_stamped.fbs deleted file mode 100644 index a4a402e69..000000000 --- a/seerep_msgs/fbs/twist_with_covariance_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "twist_with_covariance.fbs"; - -namespace seerep.fb; - -table TwistWithCovarianceStamped { - header:seerep.fb.Header; - twist:seerep.fb.TwistWithCovariance; -} diff --git a/seerep_msgs/fbs/vector3_stamped.fbs b/seerep_msgs/fbs/vector3_stamped.fbs deleted file mode 100644 index d55bde743..000000000 --- a/seerep_msgs/fbs/vector3_stamped.fbs +++ /dev/null @@ -1,10 +0,0 @@ - -include "header.fbs"; -include "vector3.fbs"; - -namespace seerep.fb; - -table Vector3Stamped { - header:seerep.fb.Header; - vector:seerep.fb.Vector3; -} diff --git a/seerep_msgs/protos/boundingbox2d.proto b/seerep_msgs/protos/boundingbox2d.proto deleted file mode 100644 index de574dbab..000000000 --- a/seerep_msgs/protos/boundingbox2d.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "point2d.proto"; - -message Boundingbox2D -{ - Point2D center_point = 1; - Point2D spatial_extent = 2; - float rotation = 3; -} diff --git a/seerep_msgs/protos/boundingbox2d_labeled.proto b/seerep_msgs/protos/boundingbox2d_labeled.proto deleted file mode 100644 index 50b6214d3..000000000 --- a/seerep_msgs/protos/boundingbox2d_labeled.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "boundingbox2d.proto"; -import "label_with_instance.proto"; - -message BoundingBox2DLabeled -{ - LabelWithInstance labelWithInstance = 1; - Boundingbox2D boundingBox = 2; -} diff --git a/seerep_msgs/protos/boundingbox2d_labeled_with_category.proto b/seerep_msgs/protos/boundingbox2d_labeled_with_category.proto deleted file mode 100644 index 31a283518..000000000 --- a/seerep_msgs/protos/boundingbox2d_labeled_with_category.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "boundingbox2d_labeled.proto"; - -message BoundingBox2DLabeledWithCategory -{ - string category = 1; - repeated BoundingBox2DLabeled boundingBox2DLabeled = 2; -} diff --git a/seerep_msgs/protos/boundingbox2d_stamped.proto b/seerep_msgs/protos/boundingbox2d_stamped.proto deleted file mode 100644 index e51d9921c..000000000 --- a/seerep_msgs/protos/boundingbox2d_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "boundingbox2d.proto"; - -message Boundingbox2DStamped -{ - Header header = 1; - Boundingbox2D boundingbox2D = 2; -} diff --git a/seerep_msgs/protos/boundingbox_labeled.proto b/seerep_msgs/protos/boundingbox_labeled.proto deleted file mode 100644 index 972db83d2..000000000 --- a/seerep_msgs/protos/boundingbox_labeled.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "boundingbox.proto"; -import "label_with_instance.proto"; - -message BoundingBoxLabeled -{ - LabelWithInstance labelWithInstance = 1; - Boundingbox boundingBox = 2; -} diff --git a/seerep_msgs/protos/boundingbox_labeled_with_category.proto b/seerep_msgs/protos/boundingbox_labeled_with_category.proto deleted file mode 100644 index 32457bf15..000000000 --- a/seerep_msgs/protos/boundingbox_labeled_with_category.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "boundingbox_labeled.proto"; - -message BoundingBoxLabeledWithCategory -{ - string category = 1; - repeated BoundingBoxLabeled boundingBoxLabeled = 2; -} diff --git a/seerep_msgs/protos/boundingbox_stamped.proto b/seerep_msgs/protos/boundingbox_stamped.proto deleted file mode 100644 index efeebdccb..000000000 --- a/seerep_msgs/protos/boundingbox_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "boundingbox.proto"; - -message BoundingboxStamped -{ - Header header = 1; - Boundingbox boundingbox = 2; -} diff --git a/seerep_msgs/protos/categories.proto b/seerep_msgs/protos/categories.proto deleted file mode 100644 index 1c6dfe0f7..000000000 --- a/seerep_msgs/protos/categories.proto +++ /dev/null @@ -1,8 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -message Categories -{ - repeated string categories = 1; -} diff --git a/seerep_msgs/protos/frame_infos.proto b/seerep_msgs/protos/frame_infos.proto deleted file mode 100644 index 7fa3409c3..000000000 --- a/seerep_msgs/protos/frame_infos.proto +++ /dev/null @@ -1,8 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -message FrameInfos -{ - repeated string frames = 1; -} diff --git a/seerep_msgs/protos/image.proto b/seerep_msgs/protos/image.proto index a28442ef4..cd43d1eec 100644 --- a/seerep_msgs/protos/image.proto +++ b/seerep_msgs/protos/image.proto @@ -3,8 +3,7 @@ syntax = "proto3"; package seerep.pb; import "header.proto"; -import "boundingbox2d_labeled_with_category.proto"; -import "labels_with_instance_with_category.proto"; +import "label_category.proto"; /** * This message contains an uncompressed image @@ -49,11 +48,8 @@ message Image bytes data = 8; // (optional) labels for the complete image - repeated LabelsWithInstanceWithCategory labels_general = 9; - - // (optional) bounding box based labels - repeated BoundingBox2DLabeledWithCategory labels_bb = 10; + repeated LabelCategory labels = 9; // camera intrinsics of the camera used to capture the image - string uuid_camera_intrinsics = 11; + string uuid_camera_intrinsics = 10; } diff --git a/seerep_msgs/protos/label.proto b/seerep_msgs/protos/label.proto index d191bd19a..b4d56e679 100644 --- a/seerep_msgs/protos/label.proto +++ b/seerep_msgs/protos/label.proto @@ -5,5 +5,7 @@ package seerep.pb; message Label { string label = 1; - float confidence = 2; + int32 labelIdDatumaro = 2; + string instanceUuid = 3; + int32 instanceIdDatumaro = 4; } diff --git a/seerep_msgs/protos/labels_with_category.proto b/seerep_msgs/protos/label_category.proto similarity index 70% rename from seerep_msgs/protos/labels_with_category.proto rename to seerep_msgs/protos/label_category.proto index 6ca0aa97f..acdd00a3d 100644 --- a/seerep_msgs/protos/labels_with_category.proto +++ b/seerep_msgs/protos/label_category.proto @@ -4,8 +4,9 @@ package seerep.pb; import "label.proto"; -message LabelsWithCategory +message LabelCategory { string category = 1; repeated Label labels = 2; + string datumaroJson = 3; } diff --git a/seerep_msgs/protos/label_with_instance.proto b/seerep_msgs/protos/label_with_instance.proto deleted file mode 100644 index c4e505e93..000000000 --- a/seerep_msgs/protos/label_with_instance.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "label.proto"; - -message LabelWithInstance -{ - Label label = 1; - string instanceUuid = 2; -} diff --git a/seerep_msgs/protos/labels.proto b/seerep_msgs/protos/labels.proto deleted file mode 100644 index 70f5c584f..000000000 --- a/seerep_msgs/protos/labels.proto +++ /dev/null @@ -1,8 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -message Labels -{ - repeated string labels = 1; -} diff --git a/seerep_msgs/protos/labels_with_instance_with_category.proto b/seerep_msgs/protos/labels_with_instance_with_category.proto deleted file mode 100644 index 43964fd3d..000000000 --- a/seerep_msgs/protos/labels_with_instance_with_category.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "label_with_instance.proto"; - -message LabelsWithInstanceWithCategory -{ - string category = 1; - repeated LabelWithInstance labelWithInstance = 2; -} diff --git a/seerep_msgs/protos/point_cloud_2.proto b/seerep_msgs/protos/point_cloud_2.proto index 74ed66568..7429e92f3 100644 --- a/seerep_msgs/protos/point_cloud_2.proto +++ b/seerep_msgs/protos/point_cloud_2.proto @@ -4,8 +4,7 @@ package seerep.pb; import "point_field.proto"; import "header.proto"; -import "boundingbox_labeled_with_category.proto"; -import "labels_with_instance_with_category.proto"; +import "label_category.proto"; /** * This message holds a collection of N-dimensional points, which may @@ -39,8 +38,5 @@ message PointCloud2 bool is_dense = 9; // (optional) labels for the complete image - repeated LabelsWithInstanceWithCategory labels_general = 10; - - // (optional) bounding box based labels - repeated BoundingBoxLabeledWithCategory labels_bb = 11; + repeated LabelCategory labels = 10; } diff --git a/seerep_msgs/protos/point_stamped.proto b/seerep_msgs/protos/point_stamped.proto index fc4dfb5d9..fca634f14 100644 --- a/seerep_msgs/protos/point_stamped.proto +++ b/seerep_msgs/protos/point_stamped.proto @@ -4,11 +4,11 @@ package seerep.pb; import "header.proto"; import "point.proto"; -import "labels_with_instance_with_category.proto"; +import "label_category.proto"; message PointStamped { Header header = 1; Point point = 2; - repeated LabelsWithInstanceWithCategory labels_general = 3; + repeated LabelCategory labels = 3; } diff --git a/seerep_msgs/protos/polygon.proto b/seerep_msgs/protos/polygon.proto deleted file mode 100644 index d95fa3124..000000000 --- a/seerep_msgs/protos/polygon.proto +++ /dev/null @@ -1,10 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "point.proto"; - -message Polygon -{ - repeated Point points = 1; -} diff --git a/seerep_msgs/protos/polygon_stamped.proto b/seerep_msgs/protos/polygon_stamped.proto deleted file mode 100644 index d8a2c49be..000000000 --- a/seerep_msgs/protos/polygon_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "polygon.proto"; - -message PolygonStamped -{ - Header header = 1; - Polygon polygon = 2; -} diff --git a/seerep_msgs/protos/pose.proto b/seerep_msgs/protos/pose.proto deleted file mode 100644 index c5c9e433b..000000000 --- a/seerep_msgs/protos/pose.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "point.proto"; -import "quaternion.proto"; - -message Pose -{ - Point position = 1; - Quaternion orientation = 2; -} diff --git a/seerep_msgs/protos/pose_stamped.proto b/seerep_msgs/protos/pose_stamped.proto deleted file mode 100644 index 70f2ece8e..000000000 --- a/seerep_msgs/protos/pose_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "pose.proto"; - -message PoseStamped -{ - Header header = 1; - Pose pose = 2; -} diff --git a/seerep_msgs/protos/quaternion_stamped.proto b/seerep_msgs/protos/quaternion_stamped.proto deleted file mode 100644 index 64b5ce6c1..000000000 --- a/seerep_msgs/protos/quaternion_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "quaternion.proto"; - -message QuaternionStamped -{ - Header header = 1; - Quaternion quaternion = 2; -} diff --git a/seerep_msgs/protos/query.proto b/seerep_msgs/protos/query.proto index b6397686e..ca0a28ee1 100644 --- a/seerep_msgs/protos/query.proto +++ b/seerep_msgs/protos/query.proto @@ -3,7 +3,7 @@ syntax = "proto3"; package seerep.pb; import "time_interval.proto"; -import "labels_with_category.proto"; +import "label_category.proto"; import "polygon2d.proto"; message Query @@ -12,7 +12,7 @@ message Query bool fullyEncapsulated = 2; bool inMapFrame = 3; TimeInterval timeinterval = 4; - repeated LabelsWithCategory labelsWithCategory = 5; + repeated LabelCategory labelCategory = 5; bool mustHaveAllLabels = 6; repeated string projectuuid = 7; repeated string instanceuuid = 8; diff --git a/seerep_msgs/protos/string_vector.proto b/seerep_msgs/protos/string_vector.proto new file mode 100644 index 000000000..3a35225d9 --- /dev/null +++ b/seerep_msgs/protos/string_vector.proto @@ -0,0 +1,8 @@ +syntax = "proto3"; + +package seerep.pb; + +message StringVector +{ + repeated string stringVector = 1; +} diff --git a/seerep_msgs/protos/twist.proto b/seerep_msgs/protos/twist.proto deleted file mode 100644 index 7ea4c47b9..000000000 --- a/seerep_msgs/protos/twist.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "vector3.proto"; - -message Twist -{ - Vector3 linear = 1; - Vector3 angular = 2; -} diff --git a/seerep_msgs/protos/twist_stamped.proto b/seerep_msgs/protos/twist_stamped.proto deleted file mode 100644 index 1fe36c817..000000000 --- a/seerep_msgs/protos/twist_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "twist.proto"; - -message TwistStamped -{ - Header header = 1; - Twist twist = 2; -} diff --git a/seerep_msgs/protos/twist_with_covariance.proto b/seerep_msgs/protos/twist_with_covariance.proto deleted file mode 100644 index a1ec36c77..000000000 --- a/seerep_msgs/protos/twist_with_covariance.proto +++ /dev/null @@ -1,20 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "twist.proto"; - -/** - * This expresses velocity in free space with uncertainty. - */ -message TwistWithCovariance -{ - Twist twist = 1; - - /* - * Row-major representation of the 6x6 covariance matrix - * The orientation parameters use a fixed-axis representation. - * In order, the parameters are: x, y, z, roll (x), pitch (y), yaw (z) - */ - repeated double covariance = 2; -} diff --git a/seerep_msgs/protos/twist_with_covariance_stamped.proto b/seerep_msgs/protos/twist_with_covariance_stamped.proto deleted file mode 100644 index fd13ca106..000000000 --- a/seerep_msgs/protos/twist_with_covariance_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "twist_with_covariance.proto"; - -message TwistWithCovarianceStamped -{ - Header header = 1; - TwistWithCovariance twist = 2; -} diff --git a/seerep_msgs/protos/vector3_stamped.proto b/seerep_msgs/protos/vector3_stamped.proto deleted file mode 100644 index 74200f259..000000000 --- a/seerep_msgs/protos/vector3_stamped.proto +++ /dev/null @@ -1,12 +0,0 @@ -syntax = "proto3"; - -package seerep.pb; - -import "header.proto"; -import "vector3.proto"; - -message Vector3Stamped -{ - Header header = 1; - Vector3 vector = 2; -} diff --git a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h index fda194b6d..a59d490d4 100644 --- a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h +++ b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h @@ -13,19 +13,15 @@ #include // seerep flatbuffer messages -#include -#include -#include #include #include #include #include #include -#include +#include #include #include #include -#include // grpc / flatbuffer #include @@ -198,59 +194,6 @@ flatbuffers::Offset toFlat(const geometry_msgs::Quaterni */ geometry_msgs::Quaternion toROS(const seerep::fb::Quaternion& quaternion); -/** - * @brief Converts a ROS geometry_msgs::Pose message to the corresponding - * gRPC Flatbuffer Pose message - * @param pose geometry_msgs::Pose - * @return gRPC Flatbuffer Pose message - */ -flatbuffers::grpc::Message toFlat(const geometry_msgs::Pose& pose); -/** - * @brief Converts a ROS geometry_msgs::Pose message to the corresponding - * Flatbuffer Pose message - * @param pose geometry_msgs::Pose - * @param builder the flatbuffer message builder to build the flatbuffer message - * @return Flatbuffer Pose message - */ -flatbuffers::Offset toFlat(const geometry_msgs::Pose& pose, - flatbuffers::grpc::MessageBuilder& builder); - -/** - * @brief Converts a Flatbuffer Pose message to the corresponding - * ROS geometry_msgs::Pose message - * @param pose Flatbuffer Pose - * @return ROS geometry_msgs::Pose - */ -geometry_msgs::Pose toROS(const seerep::fb::Pose& pose); - -/** - * @brief Converts a ROS geometry_msgs::PoseStamped message to the corresponding - * gRPC Flatbuffer PoseStamped message - * @param pose geometry_msgs::PoseStamped - * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message - * @return gRPC Flatbuffer PoseStamped message - */ -flatbuffers::grpc::Message toFlat(const geometry_msgs::PoseStamped& pose, - std::string projectuuid); -/** - * @brief Converts a ROS geometry_msgs::PoseStamped message to the corresponding - * Flatbuffer PoseStamped message - * @param pose geometry_msgs::PoseStamped - * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message - * @param builder the flatbuffer message builder to build the flatbuffer message - * @return Flatbuffer PoseStamped message - */ -flatbuffers::Offset toFlat(const geometry_msgs::PoseStamped& pose, std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder); - -/** - * @brief Converts a Flatbuffer PoseStamped message to the corresponding - * ROS geometry_msgs::PoseStamped message - * @param pose Flatbuffer PoseStamped - * @return ROS geometry_msgs::PoseStamped - */ -geometry_msgs::PoseStamped toROS(const seerep::fb::PoseStamped& pose); - /** * @brief Converts a ROS geometry_msgs::Vector3 message to the corresponding * gRPC Flatbuffer Vector3 message @@ -276,35 +219,6 @@ flatbuffers::Offset toFlat(const geometry_msgs::Vector3& ve */ geometry_msgs::Vector3 toROS(const seerep::fb::Vector3& vector); -/** - * @brief Converts a ROS geometry_msgs::Vector3Stamped message to the corresponding - * gRPC Flatbuffer Vector3Stamped message - * @param vector geometry_msgs::Vector3Stamped - * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message - * @return gRPC Flatbuffer Vector3Stamped message - */ -flatbuffers::grpc::Message toFlat(const geometry_msgs::Vector3Stamped& vector, - std::string projectuuid); -/** - * @brief Converts a ROS geometry_msgs::Vector3Stamped message to the corresponding - * Flatbuffer Vector3Stamped message - * @param vector geometry_msgs::Vector3Stamped - * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message - * @param builder the flatbuffer message builder to build the flatbuffer message - * @return Flatbuffer Vector3Stamped message - */ -flatbuffers::Offset toFlat(const geometry_msgs::Vector3Stamped& vector, - std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder); - -/** - * @brief Converts a Flatbuffer Vector3Stamped message to the corresponding - * ROS geometry_msgs::Vector3Stamped message - * @param pose Flatbuffer Vector3Stamped - * @return ROS geometry_msgs::Vector3Stamped - */ -geometry_msgs::Vector3Stamped toROS(const seerep::fb::Vector3Stamped& vector); - /** * @brief Converts a ROS geometry_msgs::Transform message to the corresponding * gRPC Flatbuffer Transform message @@ -359,66 +273,66 @@ flatbuffers::Offset toFlat(const geometry_msgs::Tr */ geometry_msgs::TransformStamped toROS(const seerep::fb::TransformStamped& transform); -/** - * @brief Converts a ROS vision_msgs::Detection2DArray message to the corresponding - * gRPC Flatbuffer BoundingBoxes2D_labeled_stamped message - * @param detection2d geometry_vision_msgs::Detection2DArray - * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message - * @param msguuid UUID of the message. Used to set the corresponding field in the Flatbuffer message - * @return gRPC Flatbuffer BoundingBoxes2D_labeled_stamped message - */ -flatbuffers::grpc::Message -toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, std::string category, - std::string msguuid = "", std::vector labels = std::vector(), - std::vector instances = std::vector()); -/** - * @brief Converts a ROS vision_msgs::Detection2DArray message to the corresponding - * Flatbuffer BoundingBoxes2D_labeled_stamped message - * @param detection2d geometry_vision_msgs::Detection2DArray - * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message - * @param builder the flatbuffer message builder to build the flatbuffer message - * @param msguuid UUID of the message. Used to set the corresponding field in the Flatbuffer message - * @return Flatbuffer BoundingBoxes2D_labeled_stamped message - */ -flatbuffers::Offset -toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder, std::string category, std::string msguuid = "", - std::vector labels = std::vector(), - std::vector instances = std::vector()); - -/** - * @brief Converts a Flatbuffer BoundingBoxes2D_labeled_stamped message to the corresponding - * ROS vision_msgs::Detection2DArray message - * @param bb_labeled_stamped Flatbuffer BoundingBoxes2D_labeled_stamped - * @return ROS vision_msgs::Detection2DArray - */ -vision_msgs::Detection2DArray toROS(const seerep::fb::BoundingBoxes2DLabeledStamped& bb_labeled_stamped); - -/** - * @brief Converts a ROS vision_msgs::Detection2D message to the corresponding - * gRPC Flatbuffer BoundingBoxes2DLabeled message - * @param detection2d geometry_vision_msgs::Detection2D - * @return gRPC Flatbuffer BoundingBoxes2DLabeled message - */ -flatbuffers::grpc::Message toFlat(const vision_msgs::Detection2D& detection2d); -/** - * @brief Converts a ROS vision_msgs::Detection2D message to the corresponding - * Flatbuffer BoundingBoxes2DLabeled message - * @param detection2d geometry_vision_msgs::Detection2D - * @param builder the flatbuffer message builder to build the flatbuffer message - * @return Flatbuffer BoundingBoxes2DLabeled message - */ -flatbuffers::Offset toFlat(const vision_msgs::Detection2D& detection2d, - flatbuffers::grpc::MessageBuilder& builder, - std::string label = "", std::string instanceUUID = ""); - -/** - * @brief Converts a Flatbuffer BoundingBoxes2DLabeled message to the corresponding - * ROS vision_msgs::Detection2D message - * @param bb_labeled_stamped Flatbuffer BoundingBoxes2DLabeled - * @return ROS vision_msgs::Detection2D - */ -vision_msgs::Detection2D toROS(const seerep::fb::BoundingBox2DLabeled& bb_labeled_stamped); +// /** +// * @brief Converts a ROS vision_msgs::Detection2DArray message to the corresponding +// * gRPC Flatbuffer BoundingBoxes2D_labeled_stamped message +// * @param detection2d geometry_vision_msgs::Detection2DArray +// * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message +// * @param msguuid UUID of the message. Used to set the corresponding field in the Flatbuffer message +// * @return gRPC Flatbuffer BoundingBoxes2D_labeled_stamped message +// */ +// flatbuffers::grpc::Message +// toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, std::string category, +// std::string msguuid = "", std::vector labels = std::vector(), +// std::vector instances = std::vector()); +// /** +// * @brief Converts a ROS vision_msgs::Detection2DArray message to the corresponding +// * Flatbuffer BoundingBoxes2D_labeled_stamped message +// * @param detection2d geometry_vision_msgs::Detection2DArray +// * @param projectuuid UUID of the target project. Used to set the corresponding field in the Flatbuffer message +// * @param builder the flatbuffer message builder to build the flatbuffer message +// * @param msguuid UUID of the message. Used to set the corresponding field in the Flatbuffer message +// * @return Flatbuffer BoundingBoxes2D_labeled_stamped message +// */ +// flatbuffers::Offset +// toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, +// flatbuffers::grpc::MessageBuilder& builder, std::string category, std::string msguuid = "", +// std::vector labels = std::vector(), +// std::vector instances = std::vector()); + +// /** +// * @brief Converts a Flatbuffer BoundingBoxes2D_labeled_stamped message to the corresponding +// * ROS vision_msgs::Detection2DArray message +// * @param bb_labeled_stamped Flatbuffer BoundingBoxes2D_labeled_stamped +// * @return ROS vision_msgs::Detection2DArray +// */ +// vision_msgs::Detection2DArray toROS(const seerep::fb::BoundingBoxes2DLabeledStamped& bb_labeled_stamped); + +// /** +// * @brief Converts a ROS vision_msgs::Detection2D message to the corresponding +// * gRPC Flatbuffer BoundingBoxes2DLabeled message +// * @param detection2d geometry_vision_msgs::Detection2D +// * @return gRPC Flatbuffer BoundingBoxes2DLabeled message +// */ +// flatbuffers::grpc::Message toFlat(const vision_msgs::Detection2D& detection2d); +// /** +// * @brief Converts a ROS vision_msgs::Detection2D message to the corresponding +// * Flatbuffer BoundingBoxes2DLabeled message +// * @param detection2d geometry_vision_msgs::Detection2D +// * @param builder the flatbuffer message builder to build the flatbuffer message +// * @return Flatbuffer BoundingBoxes2DLabeled message +// */ +// flatbuffers::Offset toFlat(const vision_msgs::Detection2D& detection2d, +// flatbuffers::grpc::MessageBuilder& builder, +// std::string label = "", std::string instanceUUID = ""); + +// /** +// * @brief Converts a Flatbuffer BoundingBoxes2DLabeled message to the corresponding +// * ROS vision_msgs::Detection2D message +// * @param bb_labeled_stamped Flatbuffer BoundingBoxes2DLabeled +// * @return ROS vision_msgs::Detection2D +// */ +// vision_msgs::Detection2D toROS(const seerep::fb::BoundingBox2DLabeled& bb_labeled_stamped); /** * @brief Converts a ROS sensor_msgs::CameraInfo message to the corresponding diff --git a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp index dc3f67aff..c3e25d5e4 100644 --- a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp +++ b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp @@ -242,64 +242,6 @@ geometry_msgs::Quaternion toROS(const seerep::fb::Quaternion& quaternion) return ret; } -/* - * Pose - */ -flatbuffers::grpc::Message toFlat(const geometry_msgs::Pose& pose) -{ - flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(pose, builder)); - return builder.ReleaseMessage(); -} -flatbuffers::Offset toFlat(const geometry_msgs::Pose& pose, flatbuffers::grpc::MessageBuilder& builder) -{ - auto position = toFlat(pose.position, builder); - auto orientation = toFlat(pose.orientation, builder); - - seerep::fb::PoseBuilder posebuilder(builder); - posebuilder.add_position(position); - posebuilder.add_orientation(orientation); - return posebuilder.Finish(); -} - -geometry_msgs::Pose toROS(const seerep::fb::Pose& pose) -{ - geometry_msgs::Pose ret; - ret.position = toROS(*pose.position()); - ret.orientation = toROS(*pose.orientation()); - return ret; -} - -/* - * PoseStamped - */ -flatbuffers::grpc::Message toFlat(const geometry_msgs::PoseStamped& pose, - std::string projectuuid) -{ - flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(pose, projectuuid, builder)); - return builder.ReleaseMessage(); -} -flatbuffers::Offset toFlat(const geometry_msgs::PoseStamped& pose, std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder) -{ - auto headerOffset = toFlat(pose.header, projectuuid, builder); - auto poseOffset = toFlat(pose.pose, builder); - - seerep::fb::PoseStampedBuilder posebuilder(builder); - posebuilder.add_header(headerOffset); - posebuilder.add_pose(poseOffset); - return posebuilder.Finish(); -} - -geometry_msgs::PoseStamped toROS(const seerep::fb::PoseStamped& pose) -{ - geometry_msgs::PoseStamped ret; - ret.header = toROS(*pose.header()); - ret.pose = toROS(*pose.pose()); - return ret; -} - /* * Vector3 */ @@ -328,36 +270,6 @@ geometry_msgs::Vector3 toROS(const seerep::fb::Vector3& vector) return ret; } -/* - * Vector3Stamped - */ -flatbuffers::grpc::Message toFlat(const geometry_msgs::Vector3Stamped& vector, - std::string projectuuid) -{ - flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(vector, projectuuid, builder)); - return builder.ReleaseMessage(); -} -flatbuffers::Offset -toFlat(const geometry_msgs::Vector3Stamped& vector, std::string projectuuid, flatbuffers::grpc::MessageBuilder& builder) -{ - auto headerOffset = toFlat(vector.header, projectuuid, builder); - auto vectorOffset = toFlat(vector.vector, builder); - - seerep::fb::Vector3StampedBuilder vector3builder(builder); - vector3builder.add_header(headerOffset); - vector3builder.add_vector(vectorOffset); - return vector3builder.Finish(); -} - -geometry_msgs::Vector3Stamped toROS(const seerep::fb::Vector3Stamped& vector) -{ - geometry_msgs::Vector3Stamped ret; - ret.header = toROS(*vector.header()); - ret.vector = toROS(*vector.vector()); - return ret; -} - /* * Transform */ @@ -422,121 +334,121 @@ geometry_msgs::TransformStamped toROS(const seerep::fb::TransformStamped& transf return ret; } -/* - * BoundingBox2DLabeledStamped - */ - -flatbuffers::grpc::Message -toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, std::string category, - std::string msguuid, std::vector labels, std::vector instances) -{ - flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(detection2d, projectuuid, builder, category, msguuid, labels, instances)); - return builder.ReleaseMessage(); -} -flatbuffers::Offset -toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, - flatbuffers::grpc::MessageBuilder& builder, std::string category, std::string msguuid, - std::vector labels, std::vector instances) -{ - // convert header - auto header = toFlat(detection2d.header, projectuuid, builder, msguuid); - - // create boundingbox labeled vector - std::vector> bblabeled; - - if (detection2d.detections.size() == labels.size() && detection2d.detections.size() == instances.size()) - { - for (long unsigned int i = 0; i < bblabeled.size(); i++) - { - bblabeled.push_back(toFlat(detection2d.detections.at(i), builder)); - } - } - else - { - // for each loop for saving in fb bb_labeled vector - for (vision_msgs::Detection2D detection : detection2d.detections) - { - bblabeled.push_back(toFlat(detection, builder)); - } - } - - // fb labels vector - auto labelsOffset = builder.CreateVector(bblabeled); - - auto categoryOffset = builder.CreateString(category); - - seerep::fb::BoundingBox2DLabeledWithCategoryBuilder bbWithCategoryBuilder(builder); - bbWithCategoryBuilder.add_category(categoryOffset); - bbWithCategoryBuilder.add_boundingBox2dLabeled(labelsOffset); - auto bbWithCategoryOffset = bbWithCategoryBuilder.Finish(); - - std::vector> bbWithCategoryVector; - bbWithCategoryVector.push_back(bbWithCategoryOffset); - auto bbWithCatbbWithCategoryVectorOffset = builder.CreateVector(bbWithCategoryVector); - - seerep::fb::BoundingBoxes2DLabeledStampedBuilder bbbuilder(builder); - bbbuilder.add_header(header); - bbbuilder.add_labels_bb(bbWithCatbbWithCategoryVectorOffset); - return bbbuilder.Finish(); -} - -/* - * BoundingBox2DLabeled - */ - -flatbuffers::grpc::Message toFlat(const vision_msgs::Detection2D& detection2d) -{ - flatbuffers::grpc::MessageBuilder builder; - builder.Finish(toFlat(detection2d, builder)); - return builder.ReleaseMessage(); -} -flatbuffers::Offset toFlat(const vision_msgs::Detection2D& detection2d, - flatbuffers::grpc::MessageBuilder& builder, - std::string label, std::string instanceUUID) -{ - seerep::fb::Point2DBuilder pointBuilderCenter(builder); - pointBuilderCenter.add_x(detection2d.bbox.center.x); - pointBuilderCenter.add_y(detection2d.bbox.center.y); - auto pointCenter = pointBuilderCenter.Finish(); - - seerep::fb::Point2DBuilder pointBuilderSpatialExtent(builder); - pointBuilderSpatialExtent.add_x(detection2d.bbox.size_x); - pointBuilderSpatialExtent.add_y(detection2d.bbox.size_y); - auto pointSpatialExtent = pointBuilderSpatialExtent.Finish(); - - seerep::fb::Boundingbox2DBuilder bbbuilder(builder); - bbbuilder.add_center_point(pointCenter); - bbbuilder.add_spatial_extent(pointSpatialExtent); - auto bb = bbbuilder.Finish(); - - flatbuffers::Offset labelOffset; - if (label == "") - { - labelOffset = builder.CreateString(std::to_string(detection2d.results.at(0).id)); - } - else - { - labelOffset = builder.CreateString(label); - } - - flatbuffers::Offset InstanceOffset = builder.CreateString(instanceUUID); - - seerep::fb::LabelBuilder labelBuilder(builder); - labelBuilder.add_label(labelOffset); - labelBuilder.add_confidence(detection2d.results.at(0).score); - auto labelMsg = labelBuilder.Finish(); - - seerep::fb::LabelWithInstanceBuilder labelInstanceBuilder(builder); - labelInstanceBuilder.add_instanceUuid(InstanceOffset); - labelInstanceBuilder.add_label(labelMsg); - auto labelWithInstanceOffset = labelInstanceBuilder.Finish(); - - seerep::fb::BoundingBox2DLabeledBuilder bblabeledbuilder(builder); - bblabeledbuilder.add_bounding_box(bb); - bblabeledbuilder.add_labelWithInstance(labelWithInstanceOffset); - return bblabeledbuilder.Finish(); -} +// /* +// * BoundingBox2DLabeledStamped +// */ + +// flatbuffers::grpc::Message +// toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, std::string category, +// std::string msguuid, std::vector labels, std::vector instances) +// { +// flatbuffers::grpc::MessageBuilder builder; +// builder.Finish(toFlat(detection2d, projectuuid, builder, category, msguuid, labels, instances)); +// return builder.ReleaseMessage(); +// } +// flatbuffers::Offset +// toFlat(const vision_msgs::Detection2DArray& detection2d, std::string projectuuid, +// flatbuffers::grpc::MessageBuilder& builder, std::string category, std::string msguuid, +// std::vector labels, std::vector instances) +// { +// // convert header +// auto header = toFlat(detection2d.header, projectuuid, builder, msguuid); + +// // create boundingbox labeled vector +// std::vector> bblabeled; + +// if (detection2d.detections.size() == labels.size() && detection2d.detections.size() == instances.size()) +// { +// for (long unsigned int i = 0; i < bblabeled.size(); i++) +// { +// bblabeled.push_back(toFlat(detection2d.detections.at(i), builder)); +// } +// } +// else +// { +// // for each loop for saving in fb bb_labeled vector +// for (vision_msgs::Detection2D detection : detection2d.detections) +// { +// bblabeled.push_back(toFlat(detection, builder)); +// } +// } + +// // fb labels vector +// auto labelsOffset = builder.CreateVector(bblabeled); + +// auto categoryOffset = builder.CreateString(category); + +// seerep::fb::BoundingBox2DLabeledWithCategoryBuilder bbWithCategoryBuilder(builder); +// bbWithCategoryBuilder.add_category(categoryOffset); +// bbWithCategoryBuilder.add_boundingBox2dLabeled(labelsOffset); +// auto bbWithCategoryOffset = bbWithCategoryBuilder.Finish(); + +// std::vector> bbWithCategoryVector; +// bbWithCategoryVector.push_back(bbWithCategoryOffset); +// auto bbWithCatbbWithCategoryVectorOffset = builder.CreateVector(bbWithCategoryVector); + +// seerep::fb::BoundingBoxes2DLabeledStampedBuilder bbbuilder(builder); +// bbbuilder.add_header(header); +// bbbuilder.add_labels_bb(bbWithCatbbWithCategoryVectorOffset); +// return bbbuilder.Finish(); +// } + +// /* +// * BoundingBox2DLabeled +// */ + +// flatbuffers::grpc::Message toFlat(const vision_msgs::Detection2D& detection2d) +// { +// flatbuffers::grpc::MessageBuilder builder; +// builder.Finish(toFlat(detection2d, builder)); +// return builder.ReleaseMessage(); +// } +// flatbuffers::Offset toFlat(const vision_msgs::Detection2D& detection2d, +// flatbuffers::grpc::MessageBuilder& builder, +// std::string label, std::string instanceUUID) +// { +// seerep::fb::Point2DBuilder pointBuilderCenter(builder); +// pointBuilderCenter.add_x(detection2d.bbox.center.x); +// pointBuilderCenter.add_y(detection2d.bbox.center.y); +// auto pointCenter = pointBuilderCenter.Finish(); + +// seerep::fb::Point2DBuilder pointBuilderSpatialExtent(builder); +// pointBuilderSpatialExtent.add_x(detection2d.bbox.size_x); +// pointBuilderSpatialExtent.add_y(detection2d.bbox.size_y); +// auto pointSpatialExtent = pointBuilderSpatialExtent.Finish(); + +// seerep::fb::Boundingbox2DBuilder bbbuilder(builder); +// bbbuilder.add_center_point(pointCenter); +// bbbuilder.add_spatial_extent(pointSpatialExtent); +// auto bb = bbbuilder.Finish(); + +// flatbuffers::Offset labelOffset; +// if (label == "") +// { +// labelOffset = builder.CreateString(std::to_string(detection2d.results.at(0).id)); +// } +// else +// { +// labelOffset = builder.CreateString(label); +// } + +// flatbuffers::Offset InstanceOffset = builder.CreateString(instanceUUID); + +// seerep::fb::LabelBuilder labelBuilder(builder); +// labelBuilder.add_label(labelOffset); +// labelBuilder.add_confidence(detection2d.results.at(0).score); +// auto labelMsg = labelBuilder.Finish(); + +// seerep::fb::LabelWithInstanceBuilder labelInstanceBuilder(builder); +// labelInstanceBuilder.add_instanceUuid(InstanceOffset); +// labelInstanceBuilder.add_label(labelMsg); +// auto labelWithInstanceOffset = labelInstanceBuilder.Finish(); + +// seerep::fb::BoundingBox2DLabeledBuilder bblabeledbuilder(builder); +// bblabeledbuilder.add_bounding_box(bb); +// bblabeledbuilder.add_labelWithInstance(labelWithInstanceOffset); +// return bblabeledbuilder.Finish(); +// } /* * camera instrinsic diff --git a/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h b/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h index 51786dde4..ecac7b1aa 100644 --- a/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h +++ b/seerep_ros/seerep_ros_conversions_pb/include/seerep_ros_conversions_pb/conversions.h @@ -2,7 +2,7 @@ #define SEEREP_ROS_CONVERSIONS_PB // ROS messages -#include +#include #include #include #include @@ -13,13 +13,12 @@ // Seerep proto messages #include #include +#include #include #include -#include #include #include #include -#include namespace seerep_ros_conversions_pb { @@ -121,38 +120,6 @@ seerep::pb::Quaternion toProto(const geometry_msgs::Quaternion& quaternion); */ geometry_msgs::Quaternion toROS(const seerep::pb::Quaternion& quaternion); -/** - * @brief Converts a ROS geometry_msgs::Pose message to the corresponding - * Protobuf Pose message - * @param pose geometry_msgs::Pose - * @return Protobuf Pose message - */ -seerep::pb::Pose toProto(const geometry_msgs::Pose& pose); - -/** - * @brief Converts a Protobuf Pose message to the corresponding - * ROS geometry_msgs::Pose message - * @param pose Protobuf Pose - * @return ROS geometry_msgs::Pose - */ -geometry_msgs::Pose toROS(const seerep::pb::Pose& pose); - -/** - * @brief Converts a ROS geometry_msgs::PoseStamped message to the corresponding - * Protobuf PoseStamped message - * @param pose geometry_msgs::PoseStamped - * @return Protobuf PoseStamped message - */ -seerep::pb::PoseStamped toProto(const geometry_msgs::PoseStamped& pose); - -/** - * @brief Converts a Protobuf PoseStamped message to the corresponding - * ROS geometry_msgs::PoseStamped message - * @param pose Protobuf PoseStamped - * @return ROS geometry_msgs::PoseStamped - */ -geometry_msgs::PoseStamped toROS(const seerep::pb::PoseStamped& pose); - /** * @brief Converts a ROS geometry_msgs::Vector3 message to the corresponding * Protobuf Vector3 message @@ -169,22 +136,6 @@ seerep::pb::Vector3 toProto(const geometry_msgs::Vector3& vector); */ geometry_msgs::Vector3 toROS(const seerep::pb::Vector3& vector); -/** - * @brief Converts a ROS geometry_msgs::Vector3Stamped message to the corresponding - * Protobuf Vector3Stamped message - * @param vector geometry_msgs::Vector3Stamped - * @return Protobuf Vector3Stamped message - */ -seerep::pb::Vector3Stamped toProto(const geometry_msgs::Vector3Stamped& vector); - -/** - * @brief Converts a Protobuf Vector3Stamped message to the corresponding - * ROS geometry_msgs::Vector3Stamped message - * @param vector Protobuf Vector3Stamped - * @return ROS geometry_msgs::Vector3Stamped - */ -geometry_msgs::Vector3Stamped toROS(const seerep::pb::Vector3Stamped& vector); - /** * @brief Converts a ROS geometry_msgs::Transform message to the corresponding * Protobuf Transform message diff --git a/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp b/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp index 0e258ed42..225ade479 100644 --- a/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp +++ b/seerep_ros/seerep_ros_conversions_pb/src/seerep_ros_conversions_pb/conversions.cpp @@ -162,44 +162,6 @@ geometry_msgs::Quaternion toROS(const seerep::pb::Quaternion& quaternion) return ret; } -/* - * Pose - */ -seerep::pb::Pose toProto(const geometry_msgs::Pose& pose) -{ - seerep::pb::Pose ret; - *ret.mutable_position() = toProto(pose.position); - *ret.mutable_orientation() = toProto(pose.orientation); - return ret; -} - -geometry_msgs::Pose toROS(const seerep::pb::Pose& pose) -{ - geometry_msgs::Pose ret; - ret.position = toROS(pose.position()); - ret.orientation = toROS(pose.orientation()); - return ret; -} - -/* - * PoseStamped - */ -seerep::pb::PoseStamped toProto(const geometry_msgs::PoseStamped& pose) -{ - seerep::pb::PoseStamped ret; - *ret.mutable_header() = toProto(pose.header); - *ret.mutable_pose() = toProto(pose.pose); - return ret; -} - -geometry_msgs::PoseStamped toROS(const seerep::pb::PoseStamped& pose) -{ - geometry_msgs::PoseStamped ret; - ret.header = toROS(pose.header()); - ret.pose = toROS(pose.pose()); - return ret; -} - /* * Vector3 */ @@ -221,25 +183,6 @@ geometry_msgs::Vector3 toROS(const seerep::pb::Vector3& vector) return ret; } -/* - * Vector3Stamped - */ -seerep::pb::Vector3Stamped toProto(const geometry_msgs::Vector3Stamped& vector) -{ - seerep::pb::Vector3Stamped ret; - *ret.mutable_header() = toProto(vector.header); - *ret.mutable_vector() = toProto(vector.vector); - return ret; -} - -geometry_msgs::Vector3Stamped toROS(const seerep::pb::Vector3Stamped& vector) -{ - geometry_msgs::Vector3Stamped ret; - ret.header = toROS(vector.header()); - ret.vector = toROS(vector.vector()); - return ret; -} - /* * Transform */ diff --git a/seerep_srv/seerep_core/include/seerep_core/core.h b/seerep_srv/seerep_core/include/seerep_core/core.h index 66707e0fa..8d74dd2ea 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core.h +++ b/seerep_srv/seerep_core/include/seerep_core/core.h @@ -73,13 +73,12 @@ class Core /** * @brief Adds labels to an existing dataset * @param datatype the targeted datatype - * @param labelWithInstancePerCategory map from category to a vector of labels to be added to the dataset + * @param labelPerCategory map from category to a vector of labels to be added to the dataset * @param msgUuid the UUID of the targeted dataset * @param projectuuid the UUID of the targeted project */ void addLabels(const seerep_core_msgs::Datatype& datatype, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid, const boost::uuids::uuid& projectuuid); /** 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 ec22fd457..5ed6793f3 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -106,24 +106,22 @@ class CoreDataset /** * @brief Adds labels to an existing dataset * @param datatype the datatype to consider - * @param labelWithInstancePerCategory map from category to a vector of labels to be added to the dataset + * @param labelPerCategory map from category to a vector of labels to be added to the dataset * @param msgUuid the UUID of the targeted dataset */ void addLabels(const seerep_core_msgs::Datatype& datatype, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid); /** * @brief Adds labels to an existing dataset * @param datatype the datatype to consider * @param datatypeSpecifics the datatypeSpecifics - * @param labelWithInstancePerCategory map from category to a vector of labels to be added to the dataset + * @param labelPerCategory map from category to a vector of labels to be added to the dataset * @param msgUuid the UUID of the targeted dataset */ void addLabels(const seerep_core_msgs::Datatype& datatype, std::shared_ptr datatypeSpecifics, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid); /** diff --git a/seerep_srv/seerep_core/include/seerep_core/core_instance.h b/seerep_srv/seerep_core/include/seerep_core/core_instance.h index d94af3867..a39fe8881 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_instance.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_instance.h @@ -6,7 +6,6 @@ // seerep-msgs #include -#include // seerep_hdf5-core #include diff --git a/seerep_srv/seerep_core/include/seerep_core/core_instances.h b/seerep_srv/seerep_core/include/seerep_core/core_instances.h index dd4a6a27f..57e9a8694 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_instances.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_instances.h @@ -9,7 +9,7 @@ // seerep-msgs #include -#include +#include // seerep_hdf5_core #include @@ -50,11 +50,10 @@ class CoreInstances std::shared_ptr createNewInstance(const std::string& label); /** * @brief Create a new instance with a given UUID and label - * @param labelWithInstance the label and the UUID of the new instance + * @param label the label and the UUID of the new instance * @return shared pointer to the created instance object */ - std::shared_ptr - createNewInstance(const seerep_core_msgs::LabelWithInstance& labelWithInstance); + std::shared_ptr createNewInstance(const seerep_core_msgs::Label& label); /** * @brief Returns the value of the attribute of the instance with the defined UUID defined by the key @@ -81,11 +80,11 @@ class CoreInstances const seerep_core_msgs::Datatype& datatype) const; /** * @brief adds an image to this instance - * @param labelWithInstance the UUID of the targeted instance and the assigned label + * @param label the UUID of the targeted instance and the assigned label * @param uuidDataset the UUID of the image * @param dataset the type of the dataset */ - void addDataset(const seerep_core_msgs::LabelWithInstance& labelWithInstance, const boost::uuids::uuid& uuidDataset, + void addDataset(const seerep_core_msgs::Label& label, const boost::uuids::uuid& uuidDataset, const seerep_core_msgs::Datatype& datatype); private: 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 a70963351..7b31ca87e 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_project.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_project.h @@ -122,12 +122,11 @@ class CoreProject /** * @brief Adds labels to an existing dataset * @param datatype the targeted datatype - * @param labelWithInstancePerCategory map from category to a vector of labels to be added to the dataset + * @param labelOfCategory map from category to a vector of labels to be added to the dataset * @param msgUuid the UUID of the targeted dataset */ void addLabels(const seerep_core_msgs::Datatype& datatype, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid); // tf diff --git a/seerep_srv/seerep_core/src/core.cpp b/seerep_srv/seerep_core/src/core.cpp index 07e087452..d44047d68 100644 --- a/seerep_srv/seerep_core/src/core.cpp +++ b/seerep_srv/seerep_core/src/core.cpp @@ -170,13 +170,12 @@ void Core::addDataset(const seerep_core_msgs::DatasetIndexable& dataset) } void Core::addLabels(const seerep_core_msgs::Datatype& datatype, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid, const boost::uuids::uuid& projectuuid) { auto project = findProject(projectuuid); - project->second->addLabels(datatype, labelWithInstancePerCategory, msgUuid); + project->second->addLabels(datatype, labelPerCategory, msgUuid); } void Core::addTF(const geometry_msgs::TransformStamped& tf, 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 92f065be3..38673b9ee 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -580,33 +580,31 @@ void CoreDataset::addDatasetToIndices(const seerep_core_msgs::Datatype& datatype ((uint64_t)dataset.header.timestamp.nanos))); datatypeSpecifics->timetree.insert(std::make_pair(aabbTime, dataset.header.uuidData)); - addLabels(datatype, datatypeSpecifics, dataset.labelsWithInstancesWithCategory, dataset.header.uuidData); + addLabels(datatype, datatypeSpecifics, dataset.labelsCategory, dataset.header.uuidData); } void CoreDataset::addLabels(const seerep_core_msgs::Datatype& datatype, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid) { - addLabels(datatype, m_datatypeDatatypeSpecificsMap.at(datatype), labelWithInstancePerCategory, msgUuid); + addLabels(datatype, m_datatypeDatatypeSpecificsMap.at(datatype), labelPerCategory, msgUuid); } void CoreDataset::addLabels(const seerep_core_msgs::Datatype& datatype, std::shared_ptr datatypeSpecifics, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid) { std::vector instanceUuids; // loop categories - for (auto labelWithInstanceOfCategory : labelWithInstancePerCategory) + for (auto labelOfCategory : labelPerCategory) { // check if label already exists - auto categoryMapEntry = datatypeSpecifics->categoryLabelDatasetsMap.find(labelWithInstanceOfCategory.first); + auto categoryMapEntry = datatypeSpecifics->categoryLabelDatasetsMap.find(labelOfCategory.first); if (categoryMapEntry == datatypeSpecifics->categoryLabelDatasetsMap.end()) { auto emplaceReturn = datatypeSpecifics->categoryLabelDatasetsMap.emplace( - labelWithInstanceOfCategory.first, std::unordered_map>()); + labelOfCategory.first, std::unordered_map>()); if (!emplaceReturn.second) { throw std::runtime_error("could not insert label category to datatype specifics."); @@ -614,7 +612,7 @@ void CoreDataset::addLabels(const seerep_core_msgs::Datatype& datatype, categoryMapEntry = emplaceReturn.first; } // loop labels - for (auto labelWithInstance : labelWithInstanceOfCategory.second) + for (auto labelWithInstance : labelOfCategory.second) { // check if label already exists auto labelMapEntry = categoryMapEntry->second.find(labelWithInstance.label); diff --git a/seerep_srv/seerep_core/src/core_instances.cpp b/seerep_srv/seerep_core/src/core_instances.cpp index 303c5e023..ce48c4c7d 100644 --- a/seerep_srv/seerep_core/src/core_instances.cpp +++ b/seerep_srv/seerep_core/src/core_instances.cpp @@ -12,17 +12,15 @@ CoreInstances::~CoreInstances() std::shared_ptr CoreInstances::createNewInstance(const std::string& label) { - return createNewInstance(seerep_core_msgs::LabelWithInstance{ - .label = label, .labelConfidence = NAN, .uuidInstance = boost::uuids::random_generator()() }); + return createNewInstance( + seerep_core_msgs::Label{ .label = label, .uuidInstance = boost::uuids::random_generator()() }); } -std::shared_ptr -CoreInstances::createNewInstance(const seerep_core_msgs::LabelWithInstance& labelWithInstance) +std::shared_ptr CoreInstances::createNewInstance(const seerep_core_msgs::Label& label) { - auto instance = - std::make_shared(seerep_core::CoreInstance(m_hdf5_io, labelWithInstance.uuidInstance)); + auto instance = std::make_shared(seerep_core::CoreInstance(m_hdf5_io, label.uuidInstance)); - instance->writeAttribute(CoreInstances::ATTRIBUTELABEL, labelWithInstance.label); - m_instances.emplace(labelWithInstance.uuidInstance, instance); + instance->writeAttribute(CoreInstances::ATTRIBUTELABEL, label.label); + m_instances.emplace(label.uuidInstance, instance); return instance; } @@ -73,21 +71,21 @@ std::vector CoreInstances::getDatasets(const std::vector instance; if (instanceMapEntry == m_instances.end()) { - instance = createNewInstance(labelWithInstance); + instance = createNewInstance(label); } else { - instance = m_instances.at(labelWithInstance.uuidInstance); + instance = m_instances.at(label.uuidInstance); } /// only add the dataset to an existing instance if the labels match otherwise throw an error - if (instance->getAttribute(CoreInstances::ATTRIBUTELABEL) == labelWithInstance.label) + if (instance->getAttribute(CoreInstances::ATTRIBUTELABEL) == label.label) { /// TODO: add categories to instances! instance->addDataset(uuidDataset, datatype); diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index a673756f9..402f1a0cd 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -139,11 +139,10 @@ void CoreProject::addDataset(const seerep_core_msgs::DatasetIndexable& dataset) } void CoreProject::addLabels(const seerep_core_msgs::Datatype& datatype, - const std::unordered_map>& - labelWithInstancePerCategory, + const std::unordered_map>& labelPerCategory, const boost::uuids::uuid& msgUuid) { - m_coreDatasets->addLabels(datatype, labelWithInstancePerCategory, msgUuid); + m_coreDatasets->addLabels(datatype, labelPerCategory, msgUuid); } void CoreProject::addTF(const geometry_msgs::TransformStamped& tf) diff --git a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h index 1becbb77b..ff8174cdf 100644 --- a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h +++ b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h @@ -11,6 +11,7 @@ #include // seerep-msgs #include +#include #include #include #include @@ -29,7 +30,7 @@ // seerep_core_msgs #include -#include +#include #include #include #include @@ -46,7 +47,7 @@ namespace seerep_core_fb { -using LabelWithInstanceVec = std::vector; +using LabelVec = std::vector; /** * @brief This class converts between flatbuffer messages and seerep core specific messages * @@ -307,25 +308,14 @@ class CoreFbConversion * @param labelsWithInstancesWithCategory the labels_general with instances (per category) in the data message in * seerep core format */ - static void fromFbDataLabelsGeneral( - const flatbuffers::Vector>* labelsGeneral, - std::unordered_map>& labelsWithInstancesWithCategory); + static void fromFbDataLabels( + const flatbuffers::Vector>* labelsGeneral, + std::unordered_map>& labelsWithInstancesWithCategory); /** * @brief converts the BoundingBox2DLabeled with instances of the flatbuffer data message to seerep core specific message * @param labelsGeneral the BoundingBox2DLabeled with instances in the flatbuffer data message * @param labelWithInstance the BoundingBox2DLabeled with instances in the data message in seerep core format */ - static void fromFbDataLabelsBb2d( - const flatbuffers::Vector>* labelsBB2d, - std::unordered_map>& labelsWithInstancesWithCategory); - /** - * @brief converts the BoundingBoxLabeled with instances of the flatbuffer data message to seerep core specific message - * @param labelsBB the BoundingBoxLabeled with instances in the flatbuffer data message - * @param labelWithInstance the BoundingBoxLabeled with instances in the data message in seerep core format - */ - static void fromFbDataLabelsBb( - const flatbuffers::Vector>* labelsBB, - std::unordered_map>& labelsWithInstancesWithCategory); }; } // namespace seerep_core_fb diff --git a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_image.h b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_image.h index 3a5a55f0d..5f7691724 100644 --- a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_image.h +++ b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_image.h @@ -72,11 +72,13 @@ class CoreFbImage * core. If the uuid of image is not defined yet, a uuid is generated and returned. */ boost::uuids::uuid addData(const seerep::fb::Image& img); - /** - * @brief Adds bounding box based labels to an existing image - * @param bbs2dlabeled the flatbuffer message containing bounding box based labels - */ - void addBoundingBoxesLabeled(const seerep::fb::BoundingBoxes2DLabeledStamped& boundingBoxes2dlabeled); + + // TODO reimplement with datumaro + // /** + // * @brief Adds bounding box based labels to an existing image + // * @param bbs2dlabeled the flatbuffer message containing bounding box based labels + // */ + // void addBoundingBoxesLabeled(const seerep::fb::BoundingBoxes2DLabeledStamped& boundingBoxes2dlabeled); private: /** @brief a shared pointer to the general core */ 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 c10d15f16..d5fb347c0 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 @@ -65,11 +65,13 @@ class CoreFbPointCloud * @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); + + // TODO reimplement with datumaro + // /** + // * @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_conversion.cpp b/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp index 8786b9e42..684fc318b 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp @@ -66,14 +66,11 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Im dataForIndices.boundingbox.max_corner().set<2>(0); // semantic - if (flatbuffers::IsFieldPresent(&img, seerep::fb::Image::VT_LABELS_GENERAL)) + if (flatbuffers::IsFieldPresent(&img, seerep::fb::Image::VT_LABELS)) { - fromFbDataLabelsGeneral(img.labels_general(), dataForIndices.labelsWithInstancesWithCategory); - } - if (flatbuffers::IsFieldPresent(&img, seerep::fb::Image::VT_LABELS_BB)) - { - fromFbDataLabelsBb2d(img.labels_bb(), dataForIndices.labelsWithInstancesWithCategory); + fromFbDataLabels(img.labels(), dataForIndices.labelsCategory); } + return dataForIndices; } seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::PointStamped* point) @@ -92,9 +89,9 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po // semantic - if (flatbuffers::IsFieldPresent(point, seerep::fb::PointStamped::VT_LABELS_GENERAL)) + if (flatbuffers::IsFieldPresent(point, seerep::fb::PointStamped::VT_LABELS)) { - fromFbDataLabelsGeneral(point->labels_general(), dataForIndices.labelsWithInstancesWithCategory); + fromFbDataLabels(point->labels(), dataForIndices.labelsCategory); } return dataForIndices; @@ -106,13 +103,9 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po fromFbDataHeader(cloud.header(), dataForIndices.header, seerep_core_msgs::Datatype::PointCloud); // semantic - if (flatbuffers::IsFieldPresent(&cloud, seerep::fb::PointCloud2::VT_LABELS_GENERAL)) - { - fromFbDataLabelsGeneral(cloud.labels_general(), dataForIndices.labelsWithInstancesWithCategory); - } - if (flatbuffers::IsFieldPresent(&cloud, seerep::fb::PointCloud2::VT_LABELS_BB)) + if (flatbuffers::IsFieldPresent(&cloud, seerep::fb::PointCloud2::VT_LABELS)) { - fromFbDataLabelsBb(cloud.labels_bb(), dataForIndices.labelsWithInstancesWithCategory); + fromFbDataLabels(cloud.labels(), dataForIndices.labelsCategory); } return dataForIndices; } @@ -601,31 +594,31 @@ boost::uuids::uuid CoreFbConversion::fromFbDataHeaderUuid(const std::string& uui } } -void CoreFbConversion::fromFbDataLabelsGeneral( - const flatbuffers::Vector>* labelsGeneral, - std::unordered_map>& labelsWithInstancesWithCategory) +void CoreFbConversion::fromFbDataLabels( + const flatbuffers::Vector>* labels, + std::unordered_map>& labelsPerCategory) { - if (labelsGeneral) + if (labels) { - for (auto labelsCategories : *labelsGeneral) + for (auto labelsCategories : *labels) { - LabelWithInstanceVec* labelWithInstanceVecPtr; + LabelVec* labelWithInstanceVecPtr; - auto catMap = labelsWithInstancesWithCategory.find(labelsCategories->category()->c_str()); - if (catMap != labelsWithInstancesWithCategory.end()) + auto catMap = labelsPerCategory.find(labelsCategories->category()->c_str()); + if (catMap != labelsPerCategory.end()) { labelWithInstanceVecPtr = &(catMap->second); } else { - std::vector labelVector; - auto entry = labelsWithInstancesWithCategory.emplace(labelsCategories->category()->c_str(), labelVector); + std::vector labelVector; + auto entry = labelsPerCategory.emplace(labelsCategories->category()->c_str(), labelVector); labelWithInstanceVecPtr = &(entry.first->second); } - if (labelsCategories->labelsWithInstance()) + if (labelsCategories->labels()) { - for (auto label : *labelsCategories->labelsWithInstance()) + for (auto label : *labelsCategories->labels()) { boost::uuids::string_generator gen; boost::uuids::uuid uuidInstance; @@ -638,55 +631,11 @@ void CoreFbConversion::fromFbDataLabelsGeneral( uuidInstance = boost::uuids::nil_uuid(); } - labelWithInstanceVecPtr->push_back( - seerep_core_msgs::LabelWithInstance{ .label = label->label()->label()->str(), - .labelConfidence = label->label()->confidence(), - .uuidInstance = uuidInstance }); - } - } - } - } -} - -void CoreFbConversion::fromFbDataLabelsBb2d( - const flatbuffers::Vector>* labelsBB2d, - std::unordered_map>& labelsWithInstancesWithCategory) -{ - if (labelsBB2d) - { - for (auto labelsCategories : *labelsBB2d) - { - std::unique_ptr labelWithInstanceVecPtr; - - auto catMap = labelsWithInstancesWithCategory.find(labelsCategories->category()->c_str()); - if (catMap != labelsWithInstancesWithCategory.end()) - { - labelWithInstanceVecPtr = std::make_unique(catMap->second); - } - else - { - std::vector labelVector; - auto entry = labelsWithInstancesWithCategory.emplace(labelsCategories->category()->c_str(), labelVector); - labelWithInstanceVecPtr = std::make_unique(entry.first->second); - } - - for (auto label : *labelsCategories->boundingBox2dLabeled()) - { - boost::uuids::string_generator gen; - boost::uuids::uuid uuidInstance; - try - { - uuidInstance = gen(label->labelWithInstance()->instanceUuid()->str()); - } - catch (std::runtime_error const& e) - { - uuidInstance = boost::uuids::nil_uuid(); + labelWithInstanceVecPtr->push_back(seerep_core_msgs::Label{ .label = label->label()->str(), + .labelIdDatumaro = label->labelIdDatumaro(), + .uuidInstance = uuidInstance, + .instanceIdDatumaro = label->labelIdDatumaro() }); } - - labelWithInstanceVecPtr->push_back( - seerep_core_msgs::LabelWithInstance{ .label = label->labelWithInstance()->label()->label()->str(), - .labelConfidence = label->labelWithInstance()->label()->confidence(), - .uuidInstance = uuidInstance }); } } } @@ -714,38 +663,6 @@ std::optional CoreFbConversion::fromFbQueryPolygon( } } -void CoreFbConversion::fromFbDataLabelsBb( - const flatbuffers::Vector>* labelsBB, - std::unordered_map>& labelsWithInstancesWithCategory) -{ - if (labelsBB) - { - for (auto labelsCategories : *labelsBB) - { - std::vector labelWithInstanceVector; - for (auto label : *labelsCategories->boundingBoxLabeled()) - { - boost::uuids::string_generator gen; - boost::uuids::uuid uuidInstance; - try - { - uuidInstance = gen(label->labelWithInstance()->instanceUuid()->str()); - } - catch (std::runtime_error const& e) - { - uuidInstance = boost::uuids::nil_uuid(); - } - - labelWithInstanceVector.push_back( - seerep_core_msgs::LabelWithInstance{ .label = label->labelWithInstance()->label()->label()->str(), - .labelConfidence = label->labelWithInstance()->label()->confidence(), - .uuidInstance = uuidInstance }); - } - labelsWithInstancesWithCategory.emplace(labelsCategories->category()->c_str(), labelWithInstanceVector); - } - } -} - std::vector CoreFbConversion::fromFbDatatypeVector(const seerep::fb::Datatype& dt) { std::vector dt_vector; diff --git a/seerep_srv/seerep_core_fb/src/core_fb_image.cpp b/seerep_srv/seerep_core_fb/src/core_fb_image.cpp index d339e7121..27ed02fd5 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_image.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_image.cpp @@ -66,48 +66,48 @@ boost::uuids::uuid CoreFbImage::addData(const seerep::fb::Image& img) } } -void CoreFbImage::addBoundingBoxesLabeled(const seerep::fb::BoundingBoxes2DLabeledStamped& boundingBoxes2dlabeled) -{ - boost::uuids::string_generator gen; - boost::uuids::uuid uuidMsg = gen(boundingBoxes2dlabeled.header()->uuid_msgs()->str()); - boost::uuids::uuid uuidProject = gen(boundingBoxes2dlabeled.header()->uuid_project()->str()); - - auto hdf5io = CoreFbGeneral::getHdf5(uuidProject, m_seerepCore, m_hdf5IoMap); - - hdf5io->writeImageBoundingBox2DLabeled(boost::lexical_cast(uuidMsg), boundingBoxes2dlabeled.labels_bb()); - - std::unordered_map> labelWithInstancePerCategory; - for (auto bbWithCategory : *boundingBoxes2dlabeled.labels_bb()) - { - if (bbWithCategory->boundingBox2dLabeled()) - { - std::vector labelsWithInstances; - for (auto bb : *bbWithCategory->boundingBox2dLabeled()) - { - 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 image in the core - // if there are already bounding box labels for this image - // 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::Image, labelWithInstancePerCategory, uuidMsg, uuidProject); - } -} +// void CoreFbImage::addBoundingBoxesLabeled(const seerep::fb::BoundingBoxes2DLabeledStamped& boundingBoxes2dlabeled) +// { +// boost::uuids::string_generator gen; +// boost::uuids::uuid uuidMsg = gen(boundingBoxes2dlabeled.header()->uuid_msgs()->str()); +// boost::uuids::uuid uuidProject = gen(boundingBoxes2dlabeled.header()->uuid_project()->str()); + +// auto hdf5io = CoreFbGeneral::getHdf5(uuidProject, m_seerepCore, m_hdf5IoMap); + +// hdf5io->writeImageBoundingBox2DLabeled(boost::lexical_cast(uuidMsg), boundingBoxes2dlabeled.labels_bb()); + +// std::unordered_map> labelWithInstancePerCategory; +// for (auto bbWithCategory : *boundingBoxes2dlabeled.labels_bb()) +// { +// if (bbWithCategory->boundingBox2dLabeled()) +// { +// std::vector labelsWithInstances; +// for (auto bb : *bbWithCategory->boundingBox2dLabeled()) +// { +// 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 image in the core +// // if there are already bounding box labels for this image +// // 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::Image, labelWithInstancePerCategory, uuidMsg, uuidProject); +// } +// } } // namespace seerep_core_fb 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 fa275907d..da59c1a9d 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_pointcloud.cpp @@ -51,48 +51,48 @@ boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pcl) 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); - } -} +// 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::Label 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_core_pb/include/seerep_core_pb/core_pb_conversion.h b/seerep_srv/seerep_core_pb/include/seerep_core_pb/core_pb_conversion.h index 4ba5268af..ab26ab63a 100644 --- a/seerep_srv/seerep_core_pb/include/seerep_core_pb/core_pb_conversion.h +++ b/seerep_srv/seerep_core_pb/include/seerep_core_pb/core_pb_conversion.h @@ -5,6 +5,7 @@ #include // seerep_msgs +#include #include #include #include @@ -19,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -38,7 +39,7 @@ namespace seerep_core_pb { -using LabelWithInstanceVec = std::vector; +using LabelVec = std::vector; /** * @brief This class converts between protobuf messages and seerep core specific messages * diff --git a/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp b/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp index ffb0d58c4..b6d65bf81 100644 --- a/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp +++ b/seerep_srv/seerep_core_pb/src/core_pb_conversion.cpp @@ -53,14 +53,14 @@ seerep_core_msgs::DatasetIndexable CorePbConversion::fromPb(const seerep::pb::Im dataForIndices.boundingbox.max_corner().set<2>(0); // semantic - if (!img.labels_general().empty()) + if (!img.labels().empty()) { - for (auto labelsCategories : img.labels_general()) + for (auto labelsCategories : img.labels()) { - std::vector labelWithInstanceVector; - if (!labelsCategories.labelwithinstance().empty()) + std::vector labelVector; + if (!labelsCategories.labels().empty()) { - for (auto label : labelsCategories.labelwithinstance()) + for (auto label : labelsCategories.labels()) { boost::uuids::string_generator gen; boost::uuids::uuid uuidInstance; @@ -73,56 +73,12 @@ seerep_core_msgs::DatasetIndexable CorePbConversion::fromPb(const seerep::pb::Im uuidInstance = boost::uuids::nil_uuid(); } - labelWithInstanceVector.push_back( - seerep_core_msgs::LabelWithInstance{ .label = label.label().label(), - .labelConfidence = label.label().confidence(), - .uuidInstance = uuidInstance }); - } - dataForIndices.labelsWithInstancesWithCategory.emplace(labelsCategories.category().c_str(), - labelWithInstanceVector); - } - } - } - - if (!img.labels_bb().empty()) - { - for (auto labelsCategories : img.labels_bb()) - { - LabelWithInstanceVec* labelWithInstanceVecPtr; - - auto catMap = dataForIndices.labelsWithInstancesWithCategory.find(labelsCategories.category().c_str()); - if (catMap != dataForIndices.labelsWithInstancesWithCategory.end()) - { - labelWithInstanceVecPtr = &(catMap->second); - } - else - { - std::vector labelVector; - auto entry = - dataForIndices.labelsWithInstancesWithCategory.emplace(labelsCategories.category().c_str(), labelVector); - labelWithInstanceVecPtr = &(entry.first->second); - } - - if (!labelsCategories.boundingbox2dlabeled().empty()) - { - for (auto label : labelsCategories.boundingbox2dlabeled()) - { - boost::uuids::string_generator gen; - boost::uuids::uuid uuidInstance; - try - { - uuidInstance = gen(label.labelwithinstance().instanceuuid()); - } - catch (std::runtime_error const& e) - { - uuidInstance = boost::uuids::nil_uuid(); - } - - labelWithInstanceVecPtr->push_back( - seerep_core_msgs::LabelWithInstance{ .label = label.labelwithinstance().label().label(), - .labelConfidence = label.labelwithinstance().label().confidence(), - .uuidInstance = uuidInstance }); + labelVector.push_back(seerep_core_msgs::Label{ .label = label.label(), + .labelIdDatumaro = label.labeliddatumaro(), + .uuidInstance = uuidInstance, + .instanceIdDatumaro = label.instanceiddatumaro() }); } + dataForIndices.labelsCategory.emplace(labelsCategories.category().c_str(), labelVector); } } } @@ -321,10 +277,10 @@ void CorePbConversion::fromPbProject(const seerep::pb::Query& query, seerep_core void CorePbConversion::fromPbLabel(const seerep::pb::Query& query, seerep_core_msgs::Query& queryCore) { - if (!query.labelswithcategory().empty()) + if (!query.labelcategory().empty()) { queryCore.label = std::unordered_map>(); - for (auto labelWithCategory : query.labelswithcategory()) + for (auto labelWithCategory : query.labelcategory()) { std::vector labels; for (auto label : labelWithCategory.labels()) diff --git a/seerep_srv/seerep_core_pb/src/core_pb_pointcloud.cpp b/seerep_srv/seerep_core_pb/src/core_pb_pointcloud.cpp index 0d15fef28..87546e79a 100644 --- a/seerep_srv/seerep_core_pb/src/core_pb_pointcloud.cpp +++ b/seerep_srv/seerep_core_pb/src/core_pb_pointcloud.cpp @@ -75,24 +75,15 @@ boost::uuids::uuid CorePbPointCloud::addData(const seerep::pb::PointCloud2& pc) dataForIndices.boundingbox.max_corner().set<2>(bb.at(5)); // semantic - int labelSizeAll = 0; - if (!pc.labels_general().empty()) - { - labelSizeAll += pc.labels_general().size(); - } - if (!pc.labels_bb().empty()) - { - labelSizeAll += pc.labels_bb().size(); - } - if (!pc.labels_general().empty()) + if (!pc.labels().empty()) { - for (auto labelsCategories : pc.labels_general()) + for (auto labelsCategories : pc.labels()) { - std::vector labelWithInstanceVector; - if (!labelsCategories.labelwithinstance().empty()) + std::vector labelWithInstanceVector; + if (!labelsCategories.labels().empty()) { - for (auto label : labelsCategories.labelwithinstance()) + for (auto label : labelsCategories.labels()) { boost::uuids::string_generator gen; boost::uuids::uuid uuidInstance; @@ -105,45 +96,13 @@ boost::uuids::uuid CorePbPointCloud::addData(const seerep::pb::PointCloud2& pc) uuidInstance = boost::uuids::nil_uuid(); } - labelWithInstanceVector.push_back( - seerep_core_msgs::LabelWithInstance{ .label = label.label().label(), - .labelConfidence = label.label().confidence(), - .uuidInstance = uuidInstance }); - } - dataForIndices.labelsWithInstancesWithCategory.emplace(labelsCategories.category().c_str(), - labelWithInstanceVector); - } - } - } - - if (!pc.labels_bb().empty()) - { - for (auto labelsCategories : pc.labels_bb()) - { - std::vector labelWithInstanceVector; - if (!labelsCategories.boundingboxlabeled().empty()) - { - for (auto label : labelsCategories.boundingboxlabeled()) - { - boost::uuids::string_generator gen; - boost::uuids::uuid uuidInstance; - try - { - uuidInstance = gen(label.labelwithinstance().instanceuuid()); - } - catch (std::runtime_error const& e) - { - uuidInstance = boost::uuids::nil_uuid(); - } - - labelWithInstanceVector.push_back( - seerep_core_msgs::LabelWithInstance{ .label = label.labelwithinstance().label().label(), - .labelConfidence = label.labelwithinstance().label().confidence(), - .uuidInstance = uuidInstance }); + labelWithInstanceVector.push_back(seerep_core_msgs::Label{ .label = label.label(), + .labelIdDatumaro = label.labeliddatumaro(), + .uuidInstance = uuidInstance, + .instanceIdDatumaro = label.instanceiddatumaro() }); } + dataForIndices.labelsCategory.emplace(labelsCategories.category().c_str(), labelWithInstanceVector); } - dataForIndices.labelsWithInstancesWithCategory.emplace(labelsCategories.category().c_str(), - labelWithInstanceVector); } } diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_image_service.h b/seerep_srv/seerep_server/include/seerep_server/fb_image_service.h index f737e021f..12ce19fa4 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_image_service.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_image_service.h @@ -24,10 +24,11 @@ class FbImageService final : public seerep::fb::ImageService::Service grpc::Status TransferImage(grpc::ServerContext* context, grpc::ServerReader>* reader, flatbuffers::grpc::Message* response) override; - grpc::Status AddBoundingBoxes2dLabeled( - grpc::ServerContext* context, - grpc::ServerReader>* reader, - flatbuffers::grpc::Message* response) override; + // TODO + // grpc::Status AddBoundingBoxes2dLabeled( + // grpc::ServerContext* context, + // grpc::ServerReader>* reader, + // flatbuffers::grpc::Message* response) override; private: std::shared_ptr imageFb; diff --git a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h index 17a3603fc..6a657220e 100644 --- a/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/fb_meta_operations.h @@ -34,10 +34,10 @@ class FbMetaOperations final : public seerep::fb::MetaOperations::Service flatbuffers::grpc::Message* response) override; grpc::Status GetAllCategories(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) override; + flatbuffers::grpc::Message* response) override; grpc::Status GetAllLabels(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) override; + flatbuffers::grpc::Message* response) override; private: std::shared_ptr seerepCore; 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 ee757fcfe..b278c8dc1 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 @@ -58,10 +58,11 @@ class FbPointCloudService final : public seerep::fb::PointCloudService::Service * @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; + // TODO + // 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/include/seerep_server/fb_tf_service.h b/seerep_srv/seerep_server/include/seerep_server/fb_tf_service.h index ee37a5aba..afb54793e 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 @@ -25,7 +25,7 @@ class FbTfService final : public seerep::fb::TfService::Service flatbuffers::grpc::Message* response); grpc::Status GetFrames(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response); + flatbuffers::grpc::Message* response); grpc::Status GetTransformStamped(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, flatbuffers::grpc::Message* response); diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h index e7937966b..a87f8582c 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_meta_operations.h @@ -28,9 +28,9 @@ class PbMetaOperations final : public seerep::pb::MetaOperations::Service grpc::Status GetOverallBoundingBox(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, seerep::pb::Boundingbox* response); grpc::Status GetAllCategories(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, - seerep::pb::Categories* response) override; + seerep::pb::StringVector* response) override; grpc::Status GetAllLabels(grpc::ServerContext* context, const seerep::pb::UuidDatatypeWithCategory* request, - seerep::pb::Labels* response); + seerep::pb::StringVector* response); private: std::shared_ptr seerepCore; diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_receive_sensor_msgs.h b/seerep_srv/seerep_server/include/seerep_server/pb_receive_sensor_msgs.h deleted file mode 100644 index 79299eed3..000000000 --- a/seerep_srv/seerep_server/include/seerep_server/pb_receive_sensor_msgs.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef SEEREP_SERVER_RECEIVE_SENSOR_MSGS_H_ -#define SEEREP_SERVER_RECEIVE_SENSOR_MSGS_H_ - -// grpc -#include -#include -#include -#include -#include - -// seerep -#include -#include - -// logging -#include -#include - -// this class is for all the gRPC calls that are not yet implemented in the server - -namespace seerep_server -{ -class PbReceiveSensorMsgs final : public seerep::pb::TransferSensorMsgs::Service -{ -public: - PbReceiveSensorMsgs(std::shared_ptr seerepCore); - grpc::Status TransferHeader(grpc::ServerContext* context, const seerep::pb::Header* header, - seerep::pb::ServerResponse* response); - - grpc::Status TransferPoint(grpc::ServerContext* context, const seerep::pb::Point* point, - seerep::pb::ServerResponse* response); - - grpc::Status TransferQuaternion(grpc::ServerContext* context, const seerep::pb::Quaternion* quaternion, - seerep::pb::ServerResponse* response); - - grpc::Status TransferPose(grpc::ServerContext* context, const seerep::pb::Pose* pose, - seerep::pb::ServerResponse* response); - - grpc::Status TransferPoseStamped(grpc::ServerContext* context, const seerep::pb::PoseStamped* pose, - seerep::pb::ServerResponse* response); - -private: - std::shared_ptr seerepCore; - boost::log::sources::severity_logger m_logger; -}; -} /* namespace seerep_server */ -#endif // SEEREP_SERVER_RECEIVE_SENSOR_MSGS_H_ diff --git a/seerep_srv/seerep_server/include/seerep_server/pb_tf_service.h b/seerep_srv/seerep_server/include/seerep_server/pb_tf_service.h index 75cc27940..d1fa11038 100644 --- a/seerep_srv/seerep_server/include/seerep_server/pb_tf_service.h +++ b/seerep_srv/seerep_server/include/seerep_server/pb_tf_service.h @@ -22,7 +22,7 @@ class PbTfService final : public seerep::pb::TfService::Service grpc::Status TransferTransformStamped(grpc::ServerContext* context, const seerep::pb::TransformStamped* transform, seerep::pb::ServerResponse* response); grpc::Status GetFrames(grpc::ServerContext* context, const seerep::pb::FrameQuery* frameQuery, - seerep::pb::FrameInfos* response); + seerep::pb::StringVector* response); grpc::Status GetTransformStamped(grpc::ServerContext* context, const seerep::pb::TransformStampedQuery* transformQuery, seerep::pb::TransformStamped* response); diff --git a/seerep_srv/seerep_server/src/fb_image_service.cpp b/seerep_srv/seerep_server/src/fb_image_service.cpp index 02e92aa20..7221505b6 100644 --- a/seerep_srv/seerep_server/src/fb_image_service.cpp +++ b/seerep_srv/seerep_server/src/fb_image_service.cpp @@ -147,68 +147,68 @@ grpc::Status FbImageService::TransferImage(grpc::ServerContext* context, return grpc::Status::OK; } -grpc::Status FbImageService::AddBoundingBoxes2dLabeled( - 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 BoundingBoxes2DLabeledStamped... "; - 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 - { - imageFb->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; -} +// grpc::Status FbImageService::AddBoundingBoxes2dLabeled( +// 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 BoundingBoxes2DLabeledStamped... +// "; 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 +// { +// imageFb->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 */ diff --git a/seerep_srv/seerep_server/src/fb_meta_operations.cpp b/seerep_srv/seerep_server/src/fb_meta_operations.cpp index 5ca57e5e9..de2b2135b 100644 --- a/seerep_srv/seerep_server/src/fb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/fb_meta_operations.cpp @@ -279,7 +279,7 @@ FbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* context, grpc::Status FbMetaOperations::GetAllCategories(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) + flatbuffers::grpc::Message* response) { (void)context; // ignore that variable without causing warnings auto requestRoot = request->GetRoot(); @@ -315,11 +315,11 @@ grpc::Status FbMetaOperations::GetAllCategories(grpc::ServerContext* context, auto fb_categories_vector = builder.CreateVector(fb_categories); - seerep::fb::CategoriesBuilder cb(builder); - cb.add_categories(fb_categories_vector); + seerep::fb::StringVectorBuilder cb(builder); + cb.add_stringVector(fb_categories_vector); builder.Finish(cb.Finish()); - *response = builder.ReleaseMessage(); + *response = builder.ReleaseMessage(); } catch (std::runtime_error const& e) { @@ -349,7 +349,7 @@ grpc::Status FbMetaOperations::GetAllCategories(grpc::ServerContext* context, grpc::Status FbMetaOperations::GetAllLabels(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) + flatbuffers::grpc::Message* response) { (void)context; // ignore that variable without causing warnings auto requestRoot = request->GetRoot(); @@ -387,11 +387,11 @@ FbMetaOperations::GetAllLabels(grpc::ServerContext* context, auto fb_labels_vector = builder.CreateVector(fb_labels); - seerep::fb::LabelsBuilder lb(builder); - lb.add_labels(fb_labels_vector); + seerep::fb::StringVectorBuilder lb(builder); + lb.add_stringVector(fb_labels_vector); builder.Finish(lb.Finish()); - *response = builder.ReleaseMessage(); + *response = builder.ReleaseMessage(); } catch (std::runtime_error const& e) { 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 f6fa7593b..112291902 100644 --- a/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp +++ b/seerep_srv/seerep_server/src/fb_point_cloud_service.cpp @@ -135,69 +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; -} +// 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 */ diff --git a/seerep_srv/seerep_server/src/fb_tf_service.cpp b/seerep_srv/seerep_server/src/fb_tf_service.cpp index ca481fdd5..9fcc4bcb5 100644 --- a/seerep_srv/seerep_server/src/fb_tf_service.cpp +++ b/seerep_srv/seerep_server/src/fb_tf_service.cpp @@ -66,7 +66,7 @@ grpc::Status FbTfService::TransferTransformStamped( grpc::Status FbTfService::GetFrames(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, - flatbuffers::grpc::Message* response) + flatbuffers::grpc::Message* response) { (void)context; // ignore that variable without causing warnings boost::uuids::uuid uuid; @@ -85,11 +85,11 @@ grpc::Status FbTfService::GetFrames(grpc::ServerContext* context, framesOffset.push_back(builder.CreateString(framename)); } - seerep::fb::FrameInfosBuilder frameinfosbuilder(builder); - frameinfosbuilder.add_frames(builder.CreateVector(framesOffset)); + seerep::fb::StringVectorBuilder frameinfosbuilder(builder); + frameinfosbuilder.add_stringVector(builder.CreateVector(framesOffset)); auto frameinfosOffset = frameinfosbuilder.Finish(); builder.Finish(frameinfosOffset); - *response = builder.ReleaseMessage(); + *response = builder.ReleaseMessage(); assert(response->Verify()); } catch (std::runtime_error const& e) diff --git a/seerep_srv/seerep_server/src/pb_meta_operations.cpp b/seerep_srv/seerep_server/src/pb_meta_operations.cpp index 9ba094fc0..98c21733c 100644 --- a/seerep_srv/seerep_server/src/pb_meta_operations.cpp +++ b/seerep_srv/seerep_server/src/pb_meta_operations.cpp @@ -179,7 +179,7 @@ grpc::Status PbMetaOperations::GetOverallBoundingBox(grpc::ServerContext* contex grpc::Status PbMetaOperations::GetAllCategories(grpc::ServerContext* context, const seerep::pb::UuidDatatypePair* request, - seerep::pb::Categories* response) + seerep::pb::StringVector* response) { (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching all categories"; @@ -197,7 +197,7 @@ grpc::Status PbMetaOperations::GetAllCategories(grpc::ServerContext* context, for (std::string category : categories) { - response->add_categories(category); + response->add_stringvector(category); } } catch (const std::exception& e) @@ -220,7 +220,7 @@ grpc::Status PbMetaOperations::GetAllCategories(grpc::ServerContext* context, grpc::Status PbMetaOperations::GetAllLabels(grpc::ServerContext* context, const seerep::pb::UuidDatatypeWithCategory* request, - seerep::pb::Labels* response) + seerep::pb::StringVector* response) { (void)context; // ignore that variable without causing warnings BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "fetching overall bounding box"; @@ -240,7 +240,7 @@ grpc::Status PbMetaOperations::GetAllLabels(grpc::ServerContext* context, for (std::string label : allLabels) { - response->add_labels(label); + response->add_stringvector(label); } } catch (const std::exception& e) diff --git a/seerep_srv/seerep_server/src/pb_receive_sensor_msgs.cpp b/seerep_srv/seerep_server/src/pb_receive_sensor_msgs.cpp deleted file mode 100644 index 559888a56..000000000 --- a/seerep_srv/seerep_server/src/pb_receive_sensor_msgs.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "seerep_server/pb_receive_sensor_msgs.h" - -namespace seerep_server -{ -PbReceiveSensorMsgs::PbReceiveSensorMsgs(std::shared_ptr seerepCore) : seerepCore(seerepCore) -{ -} - -grpc::Status PbReceiveSensorMsgs::TransferHeader(grpc::ServerContext* context, const seerep::pb::Header* header, - seerep::pb::ServerResponse* response) -{ - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "received header... "; - response->set_message("okidoki"); - response->set_transmission_state(seerep::pb::ServerResponse::SUCCESS); - return grpc::Status::OK; -} - -grpc::Status PbReceiveSensorMsgs::TransferPoint(grpc::ServerContext* context, const seerep::pb::Point* point, - seerep::pb::ServerResponse* response) -{ - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "received point... "; - // hdf5_io.writePoint("test_id", *point); - response->set_message("okidoki"); - response->set_transmission_state(seerep::pb::ServerResponse::SUCCESS); - return grpc::Status::OK; -} - -grpc::Status PbReceiveSensorMsgs::TransferQuaternion(grpc::ServerContext* context, - const seerep::pb::Quaternion* quaternion, - seerep::pb::ServerResponse* response) -{ - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "received quaternion... "; - // hdf5_io.writeQuaternion("test_id", *quaternion); - response->set_message("okidoki"); - response->set_transmission_state(seerep::pb::ServerResponse::SUCCESS); - return grpc::Status::OK; -} - -grpc::Status PbReceiveSensorMsgs::TransferPose(grpc::ServerContext* context, const seerep::pb::Pose* pose, - seerep::pb::ServerResponse* response) -{ - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "received pose... "; - // hdf5_io.writePose("test_id", *pose); - response->set_message("okidoki"); - response->set_transmission_state(seerep::pb::ServerResponse::SUCCESS); - return grpc::Status::OK; -} - -grpc::Status PbReceiveSensorMsgs::TransferPoseStamped(grpc::ServerContext* context, const seerep::pb::PoseStamped* pose, - seerep::pb::ServerResponse* response) -{ - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "received pose_stamped... "; - // hdf5_io.writePoseStamped("test_id", *pose); - response->set_message("okidoki"); - response->set_transmission_state(seerep::pb::ServerResponse::SUCCESS); - return grpc::Status::OK; -} - -} /* namespace seerep_server */ diff --git a/seerep_srv/seerep_server/src/pb_tf_service.cpp b/seerep_srv/seerep_server/src/pb_tf_service.cpp index 4651f6dca..3947404db 100644 --- a/seerep_srv/seerep_server/src/pb_tf_service.cpp +++ b/seerep_srv/seerep_server/src/pb_tf_service.cpp @@ -64,7 +64,7 @@ grpc::Status PbTfService::TransferTransformStamped(grpc::ServerContext* context, } grpc::Status PbTfService::GetFrames(grpc::ServerContext* context, const seerep::pb::FrameQuery* frameQuery, - seerep::pb::FrameInfos* response) + seerep::pb::StringVector* response) { (void)context; // ignore that variable without causing warnings boost::uuids::uuid uuid; @@ -75,7 +75,7 @@ grpc::Status PbTfService::GetFrames(grpc::ServerContext* context, const seerep:: for (auto framename : tfPb->getFrames(uuid)) { - response->add_frames(framename); + response->add_stringvector(framename); } } catch (std::runtime_error const& e)