Merge pull request #411 from agri-gaia/example/cppTfQuery

add cpp example for tf query
DFKI-NI · Aug 15, 2024 · 81cd1a4 · 81cd1a4
2 parents 7c4ecc1 + f3ed515
commit 81cd1a4
Show file tree

Hide file tree

Showing 3 changed files with 152 additions and 0 deletions.
diff --git a/examples/cpp/gRPC/CMakeLists.txt b/examples/cpp/gRPC/CMakeLists.txt
@@ -18,15 +18,24 @@ include_directories(
 )
 
 add_executable(image_query src/image_query.cpp)
+add_executable(tf_query src/tf_query.cpp)
 
 set(link_libs ${SeerepCom_LIBRARIES} ${catkin_LIBRARIES}
               gRPC::grpc++_reflection
 )
 target_link_libraries(image_query ${link_libs})
+target_link_libraries(tf_query ${link_libs})
 
 install(
   TARGETS image_query
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
 )
+
+install(
+  TARGETS tf_query
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
diff --git a/examples/cpp/gRPC/include/tf_query.h b/examples/cpp/gRPC/include/tf_query.h
@@ -0,0 +1,36 @@
+#ifndef SEEREP_CPP_EXAMPLES_TF_QUERY_H_
+#define SEEREP_CPP_EXAMPLES_TF_QUERY_H_
+
+#include <flatbuffers/grpc.h>
+#include <grpcpp/grpcpp.h>
+#include <seerep_com/meta_operations.grpc.fb.h>
+#include <seerep_com/tf_service.grpc.fb.h>
+
+#include <optional>
+
+namespace seerep_cpp_examples
+{
+
+class TfQuery
+{
+public:
+  TfQuery(std::shared_ptr<grpc::Channel> channel);
+  ~TfQuery() = default;
+  void getTf(const std::string& projectUUID, const std::string& childFrame,
+             const std::string& parentFrame, const int& seconds,
+             const int& nanos);
+
+private:
+  std::optional<std::string> getProjectUUID(const std::string& projectName);
+  flatbuffers::grpc::Message<seerep::fb::TransformStampedQuery>
+  createQuery(const std::string& projectUUID, const std::string& childFrame,
+              const std::string& parentFrame, const int& seconds,
+              const int& nanos);
+
+  std::shared_ptr<grpc::Channel> channel_;
+  flatbuffers::grpc::MessageBuilder fbb_;
+};
+
+}  // namespace seerep_cpp_examples
+
+#endif  // SEEREP_CPP_EXAMPLES_TF_QUERY_H_
diff --git a/examples/cpp/gRPC/src/tf_query.cpp b/examples/cpp/gRPC/src/tf_query.cpp
@@ -0,0 +1,107 @@
+
+#include "../include/tf_query.h"
+
+namespace seerep_cpp_examples
+{
+
+TfQuery::TfQuery(std::shared_ptr<grpc::Channel> channel)
+  : channel_(channel), fbb_(flatbuffers::grpc::MessageBuilder())
+{
+}
+
+std::optional<std::string>
+TfQuery::getProjectUUID(const std::string& projectName)
+{
+  auto metaStub = seerep::fb::MetaOperations::NewStub(channel_);
+  auto requestOffset = seerep::fb::CreateEmpty(fbb_);
+  fbb_.Finish(requestOffset);
+  auto requestMsg = fbb_.ReleaseMessage<seerep::fb::Empty>();
+
+  grpc::ClientContext context;
+  flatbuffers::grpc::Message<seerep::fb::ProjectInfos> responseMsg;
+  auto status = metaStub->GetProjects(&context, requestMsg, &responseMsg);
+
+  if (status.ok())
+  {
+    auto response = responseMsg.GetRoot();
+    for (const auto& project : *response->projects())
+    {
+      if (project->name()->str() == projectName)
+      {
+        return project->uuid()->str();
+      }
+    }
+  }
+  else
+  {
+    throw std::runtime_error(status.error_message());
+  }
+  return std::nullopt;
+}
+
+flatbuffers::grpc::Message<seerep::fb::TransformStampedQuery>
+TfQuery::createQuery(const std::string& projectUUID,
+                     const std::string& childFrame,
+                     const std::string& parentFrame, const int& seconds,
+                     const int& nanos)
+{
+  auto projectUUIDOffst = fbb_.CreateString(projectUUID);
+  auto childFrameOffset = fbb_.CreateString(childFrame);
+  auto parentFrameOffset = fbb_.CreateString(parentFrame);
+
+  seerep::fb::TimestampBuilder timestampBuilder(fbb_);
+  timestampBuilder.add_seconds(seconds);
+  timestampBuilder.add_nanos(nanos);
+  auto timestampOffset = timestampBuilder.Finish();
+
+  seerep::fb::HeaderBuilder headerBuilder(fbb_);
+  headerBuilder.add_frame_id(parentFrameOffset);
+  headerBuilder.add_stamp(timestampOffset);
+  headerBuilder.add_uuid_project(projectUUIDOffst);
+  auto headerOffset = headerBuilder.Finish();
+
+  seerep::fb::TransformStampedQueryBuilder queryBuilder(fbb_);
+  queryBuilder.add_header(headerOffset);
+  queryBuilder.add_child_frame_id(childFrameOffset);
+  /* add extra query parameters here */
+  auto requestOffset = queryBuilder.Finish();
+  fbb_.Finish(requestOffset);
+  return fbb_.ReleaseMessage<seerep::fb::TransformStampedQuery>();
+}
+
+void TfQuery::getTf(const std::string& projectName,
+                    const std::string& childFrame,
+                    const std::string& parentFrame, const int& seconds,
+                    const int& nanos)
+{
+  auto tfStub = seerep::fb::TfService::NewStub(channel_);
+  std::optional<std::string> projectUUID = getProjectUUID(projectName);
+  if (projectUUID.has_value())
+  {
+    grpc::ClientContext context;
+    flatbuffers::grpc::Message<seerep::fb::TransformStamped> responseMsg;
+    tfStub->GetTransformStamped(&context,
+                                createQuery(projectUUID.value(), childFrame,
+                                            parentFrame, seconds, nanos),
+                                &responseMsg);
+
+    auto response = responseMsg.GetRoot();
+    /* do something usefull with the images matching the query */
+    std::cout << "Received tf with childframe: "
+              << response->child_frame_id()->str() << std::endl;
+  }
+  else
+  {
+    throw std::runtime_error("Project not found");
+  }
+}
+
+}  // namespace seerep_cpp_examples
+
+int main()
+{
+  std::shared_ptr<grpc::Channel> grpcChannel =
+      grpc::CreateChannel("localhost:9090", grpc::InsecureChannelCredentials());
+  seerep_cpp_examples::TfQuery TfQuery(grpcChannel);
+  TfQuery.getTf("testproject", "map", "camera", 1661336508, 0);
+}