Add create_service() to standard bridge loader

Signed-off-by: bdm-k <kokusyunn@gmail.com>

Author: bdm-k

src/agnocastlib/include/agnocast/bridge/agnocast_bridge_node.hpp

@@ -273,9 +273,10 @@ std::shared_ptr<void> start_agno_to_ros_node(
 
 template <typename ServiceT>
 std::shared_ptr<void> start_ros_to_agno_service_node(
-  rclcpp::Node::SharedPtr node, const ServiceBridgeTargetInfo & info, const rmw_qos_profile_t & qos)
+  rclcpp::Node::SharedPtr node, const ServiceBridgeTargetInfo & info)
 {
   std::string service_name(static_cast<const char *>(info.name));
+  const rmw_qos_profile_t & qos = info.qos;
   return std::make_shared<RosToAgnocastServiceBridge<ServiceT>>(node, service_name, qos);
 }
 

src/agnocastlib/include/agnocast/bridge/standard/agnocast_standard_bridge_loader.hpp

@@ -13,6 +13,8 @@ namespace agnocast
 
 using BridgeFn = std::shared_ptr<void> (*)(
   rclcpp::Node::SharedPtr, const PubsubBridgeTargetInfo &, const rclcpp::QoS &);
+using ServiceBridgeFn =
+  std::shared_ptr<void> (*)(rclcpp::Node::SharedPtr, const ServiceBridgeTargetInfo &);
 
 class StandardBridgeLoader
 {
@@ -24,21 +26,27 @@ class StandardBridgeLoader
   StandardBridgeLoader & operator=(const StandardBridgeLoader &) = delete;
 
   std::shared_ptr<void> create(
-    const MqMsgBridge & req, const std::string & topic_name_with_direction,
+    const MqMsgBridge & req, const std::string & bridge_target_key,
     const rclcpp::Node::SharedPtr & node, const rclcpp::QoS & qos);
+  std::shared_ptr<void> create_service(
+    const MqMsgBridge & req, const std::string & bridge_target_key,
+    const rclcpp::Node::SharedPtr & node);
 
 private:
   rclcpp::Logger logger_;
 
-  std::map<std::string, std::pair<BridgeFn, std::shared_ptr<void>>> cached_factories_;
+  std::map<std::string, std::pair<uintptr_t, std::shared_ptr<void>>> cached_factories_;
 
   std::shared_ptr<void> create_bridge_instance(
     BridgeFn entry_func, const std::shared_ptr<void> & lib_handle,
     const rclcpp::Node::SharedPtr & node, const PubsubBridgeTargetInfo & target,
     const rclcpp::QoS & qos);
+  std::shared_ptr<void> create_service_bridge_instance(
+    ServiceBridgeFn entry_func, const std::shared_ptr<void> & lib_handle,
+    const rclcpp::Node::SharedPtr & node, const ServiceBridgeTargetInfo & target);
   static std::pair<void *, uintptr_t> load_library(const char * lib_path, const char * symbol_name);
-  std::pair<BridgeFn, std::shared_ptr<void>> resolve_factory_function(
-    const MqMsgBridge & req, const std::string & topic_name_with_direction);
+  std::pair<uintptr_t, std::shared_ptr<void>> resolve_factory_function(
+    const MqMsgBridge & req, const std::string & bridge_target_key);
   static bool is_address_in_library_code_segment(void * handle, uintptr_t addr);
 };
 

src/agnocastlib/src/bridge/standard/agnocast_standard_bridge_loader.cpp

@@ -25,20 +25,39 @@ StandardBridgeLoader::~StandardBridgeLoader()
 }
 
 std::shared_ptr<void> StandardBridgeLoader::create(
-  const MqMsgBridge & req, const std::string & topic_name_with_direction,
+  const MqMsgBridge & req, const std::string & bridge_target_key,
   const rclcpp::Node::SharedPtr & node, const rclcpp::QoS & qos)
 {
-  auto [entry_func, lib_handle] = resolve_factory_function(req, topic_name_with_direction);
+  auto [entry_func, lib_handle] = resolve_factory_function(req, bridge_target_key);
 
-  if (entry_func == nullptr) {
+  if (entry_func == 0) {
     const char * err = dlerror();
     RCLCPP_ERROR(
-      logger_, "Failed to resolve factory for '%s': %s", topic_name_with_direction.c_str(),
+      logger_, "Failed to resolve factory for '%s': %s", bridge_target_key.c_str(),
       err ? err : "Unknown error");
     return nullptr;
   }
 
-  return create_bridge_instance(entry_func, lib_handle, node, req.pubsub_target, qos);
+  return create_bridge_instance(
+    reinterpret_cast<BridgeFn>(entry_func), lib_handle, node, req.pubsub_target, qos);
+}
+
+std::shared_ptr<void> StandardBridgeLoader::create_service(
+  const MqMsgBridge & req, const std::string & bridge_target_key,
+  const rclcpp::Node::SharedPtr & node)
+{
+  auto [entry_func, lib_handle] = resolve_factory_function(req, bridge_target_key);
+
+  if (entry_func == 0) {
+    const char * err = dlerror();
+    RCLCPP_ERROR(
+      logger_, "Failed to resolve factory for '%s': %s", bridge_target_key.c_str(),
+      err ? err : "Unknown error");
+    return nullptr;
+  }
+
+  return create_service_bridge_instance(
+    reinterpret_cast<ServiceBridgeFn>(entry_func), lib_handle, node, req.srv_target);
 }
 
 std::shared_ptr<void> StandardBridgeLoader::create_bridge_instance(
@@ -68,6 +87,32 @@ std::shared_ptr<void> StandardBridgeLoader::create_bridge_instance(
   }
 }
 
+std::shared_ptr<void> StandardBridgeLoader::create_service_bridge_instance(
+  ServiceBridgeFn entry_func, const std::shared_ptr<void> & lib_handle,
+  const rclcpp::Node::SharedPtr & node, const ServiceBridgeTargetInfo & target)
+{
+  try {
+    auto bridge_resource = entry_func(node, target);
+    if (!bridge_resource) {
+      return nullptr;
+    }
+
+    if (lib_handle) {
+      // Prevent library unload while bridge_resource is alive (aliasing constructor)
+      using BundleType = std::pair<std::shared_ptr<void>, std::shared_ptr<void>>;
+      auto bundle = std::make_shared<BundleType>(lib_handle, bridge_resource);
+      return {bundle, bridge_resource.get()};
+    }
+
+    RCLCPP_ERROR(logger_, "Library handle is missing. Cannot ensure bridge lifetime safety.");
+    return nullptr;
+
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(logger_, "Exception in service factory: %s", e.what());
+    return nullptr;
+  }
+}
+
 std::pair<void *, uintptr_t> StandardBridgeLoader::load_library(
   const char * lib_path, const char * symbol_name)
 {
@@ -91,10 +136,10 @@ std::pair<void *, uintptr_t> StandardBridgeLoader::load_library(
   return {handle, map->l_addr};
 }
 
-std::pair<BridgeFn, std::shared_ptr<void>> StandardBridgeLoader::resolve_factory_function(
-  const MqMsgBridge & req, const std::string & topic_name_with_direction)
+std::pair<uintptr_t, std::shared_ptr<void>> StandardBridgeLoader::resolve_factory_function(
+  const MqMsgBridge & req, const std::string & bridge_target_key)
 {
-  if (auto it = cached_factories_.find(topic_name_with_direction); it != cached_factories_.end()) {
+  if (auto it = cached_factories_.find(bridge_target_key); it != cached_factories_.end()) {
     // Return the cached pair of the factory function and the shared library handle.
     return it->second;
   }
@@ -111,49 +156,47 @@ std::pair<BridgeFn, std::shared_ptr<void>> StandardBridgeLoader::resolve_factory
     if (raw_handle != nullptr) {
       dlclose(raw_handle);
     }
-    return {nullptr, nullptr};
+    return {0, nullptr};
   }
 
-  // Manage Handle Lifecycle
+  // Manage handle lifecycle
   std::shared_ptr<void> lib_handle_ptr(raw_handle, [](void * h) {
     if (h != nullptr) {
       dlclose(h);
     }
   });
 
-  // Resolve Main Function
-  uintptr_t entry_addr = base_addr + req.factory.fn_offset;
-  BridgeFn entry_func = nullptr;
-
-  if (is_address_in_library_code_segment(raw_handle, entry_addr)) {
-    entry_func = reinterpret_cast<BridgeFn>(entry_addr);
-  } else {
+  // Resolve main function
+  uintptr_t entry_func = base_addr + req.factory.fn_offset;
+  if (!is_address_in_library_code_segment(raw_handle, entry_func)) {
     RCLCPP_ERROR(
       logger_, "Main factory function pointer for '%s' is out of bounds: 0x%lx",
-      topic_name_with_direction.c_str(), static_cast<unsigned long>(entry_addr));
-    return {nullptr, nullptr};
+      bridge_target_key.c_str(), static_cast<unsigned long>(entry_func));
+    return {0, nullptr};
   }
 
-  // Register Reverse Function
+  // Service bridges do not have reverse factories yet, so early return here.
+  if (req.is_service) {
+    cached_factories_[bridge_target_key] = {entry_func, lib_handle_ptr};
+    return {entry_func, lib_handle_ptr};
+  }
+
+  // Construct reverse key
   std::string_view suffix =
     (req.direction == BridgeDirection::ROS2_TO_AGNOCAST) ? SUFFIX_A2R : SUFFIX_R2A;
-  std::string topic_name_with_reverse(static_cast<const char *>(req.pubsub_target.topic_name));
-  topic_name_with_reverse += suffix;
+  std::string reverse_key(static_cast<const char *>(req.pubsub_target.topic_name));
+  reverse_key += suffix;
 
-  uintptr_t reverse_addr = base_addr + req.factory.fn_offset_reverse;
-  BridgeFn reverse_func = nullptr;
-
-  if (is_address_in_library_code_segment(raw_handle, reverse_addr)) {
-    reverse_func = reinterpret_cast<BridgeFn>(reverse_addr);
-  } else {
+  uintptr_t reverse_func = base_addr + req.factory.fn_offset_reverse;
+  if (!is_address_in_library_code_segment(raw_handle, reverse_func)) {
     RCLCPP_ERROR(
-      logger_, "Reverse function pointer for '%s' is out of bounds: 0x%lx",
-      topic_name_with_reverse.c_str(), static_cast<unsigned long>(reverse_addr));
-    return {nullptr, nullptr};
+      logger_, "Reverse function pointer for '%s' is out of bounds: 0x%lx", reverse_key.c_str(),
+      static_cast<unsigned long>(reverse_func));
+    return {0, nullptr};
   }
 
-  cached_factories_[topic_name_with_direction] = {entry_func, lib_handle_ptr};
-  cached_factories_[topic_name_with_reverse] = {reverse_func, lib_handle_ptr};
+  cached_factories_[bridge_target_key] = {entry_func, lib_handle_ptr};
+  cached_factories_[reverse_key] = {reverse_func, lib_handle_ptr};
 
   return {entry_func, lib_handle_ptr};
 }