feat(ros2agnocast): reimplement topic info (#465)

* feat: use ioctl Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: add close fd Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
tier4 · Mar 4, 2025 · 10a5a9f · 10a5a9f
1 parent 4c83cdb
commit 10a5a9f
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 30 deletions.
diff --git a/src/agnocast_ioctl_wrapper/src/agnocast_ioctl.hpp b/src/agnocast_ioctl_wrapper/src/agnocast_ioctl.hpp
@@ -46,5 +46,7 @@ union ioctl_topic_info_args {
 #pragma GCC diagnostic pop
 
 #define AGNOCAST_GET_TOPIC_LIST_CMD _IOR('R', 1, union ioctl_topic_list_args)
-#define AGNOCAST_GET_NODE_SUBSCRIBER_TOPICS_CMD _IOR('R', 2, union ioctl_node_info_args)
-#define AGNOCAST_GET_NODE_PUBLISHER_TOPICS_CMD _IOR('R', 3, union ioctl_node_info_args)
+#define AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD _IOR('R', 2, union ioctl_topic_info_args)
+#define AGNOCAST_GET_TOPIC_PUBLISHER_INFO_CMD _IOR('R', 3, union ioctl_topic_info_args)
+#define AGNOCAST_GET_NODE_SUBSCRIBER_TOPICS_CMD _IOR('R', 4, union ioctl_node_info_args)
+#define AGNOCAST_GET_NODE_PUBLISHER_TOPICS_CMD _IOR('R', 5, union ioctl_node_info_args)
diff --git a/src/agnocast_ioctl_wrapper/src/topic_info.cpp b/src/agnocast_ioctl_wrapper/src/topic_info.cpp
@@ -15,61 +15,79 @@ struct topic_info_ret * get_agnocast_sub_nodes(const char * topic_name, int * to
 {
   *topic_info_ret_count = 0;
 
+  int fd = open("/dev/agnocast", O_RDONLY);
+  if (fd < 0) {
+    perror("Failed to open /dev/agnocast");
+    return nullptr;
+  }
+
   struct topic_info_ret * agnocast_topic_info_ret_buffer = static_cast<struct topic_info_ret *>(
     malloc(MAX_TOPIC_INFO_RET_NUM * sizeof(struct topic_info_ret)));
 
   if (agnocast_topic_info_ret_buffer == nullptr) {
     fprintf(stderr, "Memory allocation failed\n");
+    close(fd);
     return nullptr;
   }
 
-  ////FIXME: Replace this code to calling agnocast ////
-  const char * nodes[] = {"/listener_node", "/tmp/node_B", "/tmp/temporary/node_C"};
-  size_t num_nodes = 3;
-
-  for (size_t i = 0; i < num_nodes; i++) {
-    strncpy(
-      agnocast_topic_info_ret_buffer[i].node_name, nodes[i],
-      sizeof(agnocast_topic_info_ret_buffer[i].node_name) - 1);
-    agnocast_topic_info_ret_buffer[i]
-      .node_name[sizeof(agnocast_topic_info_ret_buffer[i].node_name) - 1] = '\0';
-    agnocast_topic_info_ret_buffer[i].qos_depth = 3;
-    agnocast_topic_info_ret_buffer[i].qos_is_transient_local = true;
+  union ioctl_topic_info_args topic_info_args = {};
+  topic_info_args.topic_name = topic_name;
+  topic_info_args.topic_info_ret_buffer_addr =
+    reinterpret_cast<uint64_t>(agnocast_topic_info_ret_buffer);
+  if (ioctl(fd, AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD, &topic_info_args) < 0) {
+    perror("AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD failed");
+    free(agnocast_topic_info_ret_buffer);
+    close(fd);
+    return nullptr;
+  }
+
+  if (topic_info_args.ret_topic_info_ret_num == 0) {
+    free(agnocast_topic_info_ret_buffer);
+    close(fd);
+    return nullptr;
   }
-  ////////////////////////////////////////////////////
 
-  *topic_info_ret_count = static_cast<int>(num_nodes);
+  *topic_info_ret_count = static_cast<int>(topic_info_args.ret_topic_info_ret_num);
   return agnocast_topic_info_ret_buffer;
 }
 
 struct topic_info_ret * get_agnocast_pub_nodes(const char * topic_name, int * topic_info_ret_count)
 {
   *topic_info_ret_count = 0;
 
+  int fd = open("/dev/agnocast", O_RDONLY);
+  if (fd < 0) {
+    perror("Failed to open /dev/agnocast");
+    return nullptr;
+  }
+
   struct topic_info_ret * agnocast_topic_info_ret_buffer = static_cast<struct topic_info_ret *>(
     malloc(MAX_TOPIC_INFO_RET_NUM * sizeof(struct topic_info_ret)));
 
   if (agnocast_topic_info_ret_buffer == nullptr) {
     fprintf(stderr, "Memory allocation failed\n");
+    close(fd);
     return nullptr;
   }
 
-  ////FIXME: Replace this code to calling agnocast ////
-  const char * nodes[] = {"/talker_node", "/tmp/node_B", "/tmp/temporary/node_C"};
-  size_t num_nodes = 3;
-
-  for (size_t i = 0; i < num_nodes; i++) {
-    strncpy(
-      agnocast_topic_info_ret_buffer[i].node_name, nodes[i],
-      sizeof(agnocast_topic_info_ret_buffer[i].node_name) - 1);
-    agnocast_topic_info_ret_buffer[i]
-      .node_name[sizeof(agnocast_topic_info_ret_buffer[i].node_name) - 1] = '\0';
-    agnocast_topic_info_ret_buffer[i].qos_depth = 3;
-    agnocast_topic_info_ret_buffer[i].qos_is_transient_local = true;
+  union ioctl_topic_info_args topic_info_args = {};
+  topic_info_args.topic_name = topic_name;
+  topic_info_args.topic_info_ret_buffer_addr =
+    reinterpret_cast<uint64_t>(agnocast_topic_info_ret_buffer);
+  if (ioctl(fd, AGNOCAST_GET_TOPIC_PUBLISHER_INFO_CMD, &topic_info_args) < 0) {
+    perror("AGNOCAST_GET_TOPIC_PUBLISHER_INFO_CMD failed");
+    free(agnocast_topic_info_ret_buffer);
+    close(fd);
+    return nullptr;
+  }
+
+  if (topic_info_args.ret_topic_info_ret_num == 0) {
+    free(agnocast_topic_info_ret_buffer);
+    close(fd);
+    return nullptr;
   }
-  ////////////////////////////////////////////////////
 
-  *topic_info_ret_count = static_cast<int>(num_nodes);
+  *topic_info_ret_count = static_cast<int>(topic_info_args.ret_topic_info_ret_num);
   return agnocast_topic_info_ret_buffer;
 }