feat(ros2agnocast): add topic info cmd base (#442)

* feat: add base Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: add comment Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: change topic info ret Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: response to cppcheck Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: remove topic list Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: remove empty line Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: decrease node name buffer size Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: change display format Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: move getting ros2 topic list Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: display format mistake Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: remove null check before free Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: move print to another functions Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: MAX_TOPIC_INFO_RET_NUM Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: include algorithm and std::max Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: copy latest MAX_PUBLISHER/SUBSCRIBER_NUM Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: add none check Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: remove unnecessary arg Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * 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> * feat(kmod): add get topic pub sub cmd (#457) * feat: add topic info cmds Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: use MAX_TOPIC_INFO_RET_NUM Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: redefine MAX_TOPIC_INFO_RET_NUM Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: once copy_to_usr Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: response to cppcheck Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: add kfree Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * tmp: write space Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: pre-commit Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
tier4 · Mar 5, 2025 · ad08fce · ad08fce
1 parent 5be5b36
commit ad08fce
Show file tree

Hide file tree

Showing 7 changed files with 465 additions and 10 deletions.
diff --git a/agnocast_kmod/agnocast.c b/agnocast_kmod/agnocast.c
@@ -2,6 +2,7 @@
 
 #include <linux/device.h>
 #include <linux/hashtable.h>
+#include <linux/kernel.h>
 #include <linux/kprobes.h>
 #include <linux/kthread.h>
 #include <linux/slab.h>  // kmalloc, kfree
@@ -31,7 +32,9 @@ static DEFINE_MUTEX(global_mutex);
 
 // Maximum length of topic name: 256 characters
 #define TOPIC_NAME_BUFFER_SIZE 256
-#define NODE_NAME_BUFFER_SIZE 256
+
+// Maximum number of topic info ret
+#define MAX_TOPIC_INFO_RET_NUM max(MAX_PUBLISHER_NUM, MAX_SUBSCRIBER_NUM)
 
 struct process_info
 {
@@ -1401,6 +1404,122 @@ static int get_node_publisher_topics(
   return 0;
 }
 
+static int get_topic_subscriber_info(
+  const char * topic_name, union ioctl_topic_info_args * topic_info_args)
+{
+  topic_info_args->ret_topic_info_ret_num = 0;
+
+  struct topic_wrapper * wrapper = find_topic(topic_name);
+  if (!wrapper) {
+    return 0;
+  }
+
+  uint32_t subscriber_num = 0;
+  struct subscriber_info * sub_info;
+  int bkt_sub_info;
+
+  struct topic_info_ret __user * user_buffer =
+    (struct topic_info_ret *)topic_info_args->topic_info_ret_buffer_addr;
+
+  struct topic_info_ret * topic_info_mem =
+    kmalloc(sizeof(struct topic_info_ret) * MAX_TOPIC_INFO_RET_NUM, GFP_KERNEL);
+  if (!topic_info_mem) {
+    return -ENOMEM;
+  }
+
+  hash_for_each(wrapper->topic.sub_info_htable, bkt_sub_info, sub_info, node)
+  {
+    if (subscriber_num >= MAX_TOPIC_INFO_RET_NUM) {
+      dev_warn(
+        agnocast_device, "The number of subscribers is over MAX_TOPIC_INFO_RET_NUM=%d\n",
+        MAX_TOPIC_INFO_RET_NUM);
+      kfree(topic_info_mem);
+      return -ENOBUFS;
+    }
+
+    if (!sub_info->node_name) {
+      kfree(topic_info_mem);
+      return -EFAULT;
+    }
+
+    struct topic_info_ret * temp_info = &topic_info_mem[subscriber_num];
+
+    strncpy(temp_info->node_name, sub_info->node_name, strlen(sub_info->node_name));
+    temp_info->qos_depth = sub_info->qos_depth;
+    temp_info->qos_is_transient_local = sub_info->qos_is_transient_local;
+
+    subscriber_num++;
+  }
+
+  if (copy_to_user(user_buffer, topic_info_mem, sizeof(struct topic_info_ret) * subscriber_num)) {
+    kfree(topic_info_mem);
+    return -EFAULT;
+  }
+
+  kfree(topic_info_mem);
+  topic_info_args->ret_topic_info_ret_num = subscriber_num;
+
+  return 0;
+}
+
+static int get_topic_publisher_info(
+  const char * topic_name, union ioctl_topic_info_args * topic_info_args)
+{
+  topic_info_args->ret_topic_info_ret_num = 0;
+
+  struct topic_wrapper * wrapper = find_topic(topic_name);
+  if (!wrapper) {
+    return 0;
+  }
+
+  uint32_t publisher_num = 0;
+  struct publisher_info * pub_info;
+  int bkt_pub_info;
+
+  struct topic_info_ret __user * user_buffer =
+    (struct topic_info_ret *)topic_info_args->topic_info_ret_buffer_addr;
+
+  struct topic_info_ret * topic_info_mem =
+    kmalloc(sizeof(struct topic_info_ret) * MAX_TOPIC_INFO_RET_NUM, GFP_KERNEL);
+  if (!topic_info_mem) {
+    return -ENOMEM;
+  }
+
+  hash_for_each(wrapper->topic.pub_info_htable, bkt_pub_info, pub_info, node)
+  {
+    if (publisher_num >= MAX_TOPIC_INFO_RET_NUM) {
+      dev_warn(
+        agnocast_device, "The number of publishers is over MAX_TOPIC_INFO_RET_NUM=%d\n",
+        MAX_TOPIC_INFO_RET_NUM);
+      kfree(topic_info_mem);
+      return -ENOBUFS;
+    }
+
+    if (!pub_info->node_name) {
+      kfree(topic_info_mem);
+      return -EFAULT;
+    }
+
+    struct topic_info_ret * temp_info = &topic_info_mem[publisher_num];
+
+    strncpy(temp_info->node_name, pub_info->node_name, strlen(pub_info->node_name));
+    temp_info->qos_depth = pub_info->qos_depth;
+    temp_info->qos_is_transient_local = pub_info->qos_is_transient_local;
+
+    publisher_num++;
+  }
+
+  if (copy_to_user(user_buffer, topic_info_mem, sizeof(struct topic_info_ret) * publisher_num)) {
+    kfree(topic_info_mem);
+    return -EFAULT;
+  }
+
+  kfree(topic_info_mem);
+  topic_info_args->ret_topic_info_ret_num = publisher_num;
+
+  return 0;
+}
+
 static long agnocast_ioctl(struct file * file, unsigned int cmd, unsigned long arg)
 {
   mutex_lock(&global_mutex);
@@ -1559,6 +1678,36 @@ static long agnocast_ioctl(struct file * file, unsigned int cmd, unsigned long a
           (union ioctl_node_info_args __user *)arg, &node_info_pub_args,
           sizeof(node_info_pub_args)))
       goto unlock_mutex_and_return;
+  } else if (cmd == AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD) {
+    char topic_name_buf[TOPIC_NAME_BUFFER_SIZE];
+    union ioctl_topic_info_args topic_info_sub_args;
+    if (copy_from_user(
+          &topic_info_sub_args, (union ioctl_topic_info_args __user *)arg,
+          sizeof(topic_info_sub_args)))
+      goto unlock_mutex_and_return;
+    if (copy_from_user(
+          topic_name_buf, (char __user *)topic_info_sub_args.topic_name, sizeof(topic_name_buf)))
+      goto unlock_mutex_and_return;
+    ret = get_topic_subscriber_info(topic_name_buf, &topic_info_sub_args);
+    if (copy_to_user(
+          (union ioctl_topic_info_args __user *)arg, &topic_info_sub_args,
+          sizeof(topic_info_sub_args)))
+      goto unlock_mutex_and_return;
+  } else if (cmd == AGNOCAST_GET_TOPIC_PUBLISHER_INFO_CMD) {
+    char topic_name_buf[TOPIC_NAME_BUFFER_SIZE];
+    union ioctl_topic_info_args topic_info_pub_args;
+    if (copy_from_user(
+          &topic_info_pub_args, (union ioctl_topic_info_args __user *)arg,
+          sizeof(topic_info_pub_args)))
+      goto unlock_mutex_and_return;
+    if (copy_from_user(
+          topic_name_buf, (char __user *)topic_info_pub_args.topic_name, sizeof(topic_name_buf)))
+      goto unlock_mutex_and_return;
+    ret = get_topic_publisher_info(topic_name_buf, &topic_info_pub_args);
+    if (copy_to_user(
+          (union ioctl_topic_info_args __user *)arg, &topic_info_pub_args,
+          sizeof(topic_info_pub_args)))
+      goto unlock_mutex_and_return;
   } else {
     mutex_unlock(&global_mutex);
     return -EINVAL;

diff --git a/agnocast_kmod/agnocast.h b/agnocast_kmod/agnocast.h
@@ -2,10 +2,11 @@
 
 #include <linux/types.h>
 
-#define MAX_PUBLISHER_NUM 4    // Maximum number of publishers per topic
-#define MAX_SUBSCRIBER_NUM 16  // Maximum number of subscribers per topic
-#define MAX_QOS_DEPTH 10       // Maximum QoS depth for each publisher/subscriber
-#define MAX_RELEASE_NUM 3      // Maximum number of entries that can be released at one ioctl
+#define MAX_PUBLISHER_NUM 4        // Maximum number of publishers per topic
+#define MAX_SUBSCRIBER_NUM 16      // Maximum number of subscribers per topic
+#define MAX_QOS_DEPTH 10           // Maximum QoS depth for each publisher/subscriber
+#define MAX_RELEASE_NUM 3          // Maximum number of entries that can be released at one ioctl
+#define NODE_NAME_BUFFER_SIZE 256  // Maximum length of node name: 256 characters
 
 typedef int32_t topic_local_id_t;
 struct publisher_shm_info
@@ -148,9 +149,27 @@ union ioctl_node_info_args {
   uint32_t ret_topic_num;
 };
 
+struct topic_info_ret
+{
+  char node_name[NODE_NAME_BUFFER_SIZE];
+  uint32_t qos_depth;
+  bool qos_is_transient_local;
+};
+
+union ioctl_topic_info_args {
+  struct
+  {
+    const char * topic_name;
+    uint64_t topic_info_ret_buffer_addr;
+  };
+  uint32_t ret_topic_info_ret_num;
+};
+
 #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)
 
 // ================================================
 // public functions in agnocast.c

diff --git a/src/agnocast_ioctl_wrapper/CMakeLists.txt b/src/agnocast_ioctl_wrapper/CMakeLists.txt
@@ -4,7 +4,7 @@ project(agnocast_ioctl_wrapper)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 
-add_library(agnocast_ioctl_wrapper SHARED src/topic_list.cpp src/node_info.cpp)
+add_library(agnocast_ioctl_wrapper SHARED src/topic_list.cpp src/node_info.cpp src/topic_info.cpp)
 
 ament_target_dependencies(agnocast_ioctl_wrapper rclcpp)
 

diff --git a/src/agnocast_ioctl_wrapper/src/agnocast_ioctl.hpp b/src/agnocast_ioctl_wrapper/src/agnocast_ioctl.hpp
@@ -1,9 +1,16 @@
 #pragma once
 
+#include <algorithm>
 #include <cstdint>
 
+#define MAX_PUBLISHER_NUM 4    // Maximum number of publishers per topic
+#define MAX_SUBSCRIBER_NUM 16  // Maximum number of subscribers per topic
+
 #define MAX_TOPIC_NUM 1024
+#define MAX_TOPIC_INFO_RET_NUM std::max(MAX_PUBLISHER_NUM, MAX_SUBSCRIBER_NUM)
+
 #define TOPIC_NAME_BUFFER_SIZE 256
+#define NODE_NAME_BUFFER_SIZE 256
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -20,8 +27,26 @@ union ioctl_node_info_args {
   };
   uint32_t ret_topic_num;
 };
+
+struct topic_info_ret
+{
+  char node_name[NODE_NAME_BUFFER_SIZE];
+  uint32_t qos_depth;
+  bool qos_is_transient_local;
+};
+
+union ioctl_topic_info_args {
+  struct
+  {
+    const char * topic_name;
+    uint64_t topic_info_ret_buffer_addr;
+  };
+  uint32_t ret_topic_info_ret_num;
+};
 #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
@@ -0,0 +1,98 @@
+#include "agnocast_ioctl.hpp"
+
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include <cstdlib>
+#include <cstring>
+#include <string>
+#include <vector>
+
+extern "C" {
+
+struct topic_info_ret * get_agnocast_sub_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;
+  }
+
+  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>(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;
+  }
+
+  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>(topic_info_args.ret_topic_info_ret_num);
+  return agnocast_topic_info_ret_buffer;
+}
+
+void free_agnocast_topic_info_ret(struct topic_info_ret * array)
+{
+  free(array);
+}
+}