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