Skip to content

Commit

Permalink
minor optimizations; more logging
Browse files Browse the repository at this point in the history
  • Loading branch information
mmeijerdfki committed Aug 6, 2024
1 parent 053cfa7 commit 42dff17
Show file tree
Hide file tree
Showing 7 changed files with 57 additions and 71 deletions.
4 changes: 1 addition & 3 deletions examples/python/gRPC/images/gRPC_fb_queryImages.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
49 changes: 9 additions & 40 deletions examples/python/gRPC/tf/gRPC_fb_deleteTfs.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -22,6 +19,7 @@
createTransformStamped,
createTransformStampedQueryInterval,
createVector3,
getOrCreateProject,
)
from seerep.util.fb_to_dict import SchemaFileNames, fb_flatc_dict

Expand All @@ -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)

Expand Down Expand Up @@ -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
Expand All @@ -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")
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
30 changes: 18 additions & 12 deletions seerep_hdf5/seerep_hdf5_fb/src/hdf5_fb_tf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<HighFive::Group> group_ptr =
std::make_shared<HighFive::Group>(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);

Expand Down Expand Up @@ -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<std::vector<int64_t>> reduced_time;
std::vector<std::vector<double>> reduced_trans;
std::vector<std::vector<double>> 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)...";

Expand Down Expand Up @@ -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;
Expand Down
9 changes: 5 additions & 4 deletions seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_tf.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<boost::uuids::uuid>
deleteHdf5(const seerep::fb::TransformStampedIntervalQuery& tfInterval);

/**
Expand Down
25 changes: 17 additions & 8 deletions seerep_srv/seerep_core_fb/src/core_fb_tf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ CoreFbTf::getFrames(const boost::uuids::uuid& projectuuid)
return m_seerepCore->getFrames(projectuuid);
}

boost::uuids::uuid
std::optional<boost::uuids::uuid>
CoreFbTf::deleteHdf5(const seerep::fb::TransformStampedIntervalQuery& tfInterval)
{
auto projectuuid = boost::lexical_cast<boost::uuids::uuid>(
Expand All @@ -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
Expand Down
6 changes: 4 additions & 2 deletions seerep_srv/seerep_server/src/fb_tf_service.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit 42dff17

Please sign in to comment.