feat(ros2agnocast): add topic info cmd base

ros2agnocast/ros2agnocast/verb/topic_info_agnocast.py

@@ -0,0 +1,156 @@
+import ctypes
+import sys
+from ros2cli.node.strategy import add_arguments as add_strategy_node_arguments
+from ros2cli.node.strategy import NodeStrategy
+from ros2topic.api import get_topic_names_and_types
+from ros2topic.api import TopicNameCompleter
+from ros2node.verb import VerbExtension
+
+class TopicInfoRet(ctypes.Structure):
+    _fields_ = [
+        ("node_name", ctypes.c_char * 256),
+        ("qos_depth", ctypes.c_uint32),
+        ("qos_is_transient_local", ctypes.c_bool),
+    ]
+
+class TopicInfoAgnocastVerb(VerbExtension):
+    """Print information about a topic including Agnocast."""
+
+    def add_arguments(self, parser, cli_name):
+        add_strategy_node_arguments(parser)
+        arg = parser.add_argument(
+            'topic_name',
+            help="Name of the ROS topic to get info (e.g. '/chatter') including Agnocast.")
+        parser.add_argument(
+            '--verbose',
+            '-v',
+            action='store_true',
+            help='Prints detailed information like the node name, node namespace, topic type, '
+                 'topic type hash, GUID, and QoS Profile of the publishers and subscribers to '
+                 'this topic. In case of Agnocast, only the supported QoS parameters are '
+                 'displayed')
+        arg.completer = TopicNameCompleter(
+            include_hidden_topics_key='include_hidden_topics')
+
+    def split_full_node_name(self, full_node_name):
+        full_node_name = full_node_name.rstrip("/") if full_node_name != "/" else full_node_name
+        namespace, _, node_name = full_node_name.rpartition("/")
+        namespace = namespace if namespace else "/"
+        return namespace, node_name
+
+    def main(self, *, args):
+        with NodeStrategy(None) as node:
+            lib = ctypes.CDLL("libagnocast_ioctl_wrapper.so")
+            lib.get_agnocast_sub_nodes.argtypes = [ctypes.c_char_p, ctypes.POINTER(ctypes.c_int)]
+            lib.get_agnocast_sub_nodes.restype = ctypes.POINTER(TopicInfoRet)
+            lib.get_agnocast_pub_nodes.argtypes = [ctypes.c_char_p, ctypes.POINTER(ctypes.c_int)]
+            lib.get_agnocast_pub_nodes.restype = ctypes.POINTER(TopicInfoRet)          
+            lib.free_agnocast_topic_info_ret.argtypes = [ctypes.POINTER(TopicInfoRet), ctypes.c_int]
+            lib.free_agnocast_topic_info_ret.restype = None
+
+            # get ros2 topic list
+            topic_names_and_types = get_topic_names_and_types(node=node, include_hidden_topics=True)
+
+            topic_name = args.topic_name
+            topic_name_byte = topic_name.encode('utf-8')
+            topic_types = None
+            for (t_name, t_types) in topic_names_and_types:
+                if t_name == topic_name:
+                    topic_types = t_types
+                    break
+
+            # get agnocast sub node list
+            sub_topic_info_ret_count = ctypes.c_int()
+            sub_topic_info_ret_array = lib.get_agnocast_sub_nodes(topic_name_byte, ctypes.byref(sub_topic_info_ret_count))
+            sub_topic_info_rets = []
+            for i in range(sub_topic_info_ret_count.value):
+                sub_topic_info_rets.append({
+                    "node_name": sub_topic_info_ret_array[i].node_name.decode('utf-8'),
+                    "qos_depth": sub_topic_info_ret_array[i].qos_depth,
+                    "qos_is_transient_local": sub_topic_info_ret_array[i].qos_is_transient_local,
+                })
+            if sub_topic_info_ret_count.value != 0:
+                lib.free_agnocast_topic_info_ret(sub_topic_info_ret_array, sub_topic_info_ret_count)
+
+            # get agnocast pub node list
+            pub_topic_info_ret_count = ctypes.c_int()
+            pub_topic_info_ret_array = lib.get_agnocast_pub_nodes(topic_name_byte, ctypes.byref(pub_topic_info_ret_count))
+            pub_topic_info_rets = []
+            for i in range(pub_topic_info_ret_count.value):
+                pub_topic_info_rets.append({
+                    "node_name": pub_topic_info_ret_array[i].node_name.decode('utf-8'),
+                    "qos_depth": pub_topic_info_ret_array[i].qos_depth,
+                    "qos_is_transient_local": pub_topic_info_ret_array[i].qos_is_transient_local,
+                })
+            if pub_topic_info_ret_count.value != 0:
+                lib.free_agnocast_topic_info_ret(pub_topic_info_ret_array, pub_topic_info_ret_count)
+
+            # check if topic exists
+            if topic_types is None:
+                if sub_topic_info_ret_count.value == 0 and pub_topic_info_ret_count.value == 0:
+                    return 'Unkown topic: %s' % topic_name
+                else:
+                    topic_types = '<UNKNOWN>'
+
+            ########################################################################
+            # Temp Code: existing ros2 topic info functionality
+            ########################################################################
+            line_end = '\n'
+            if args.verbose:
+                line_end = '\n\n'
+            type_str = topic_types[0] if len(topic_types) == 1 else topic_types
+            print('Type: %s' % type_str, end=line_end)
+
+            agnocast_pub_count = len(pub_topic_info_rets)
+            print('ROS 2 Publisher count: %d' %
+                  node.count_publishers(topic_name))
+            print('Agnocast Publisher count: %d)' %
+                  agnocast_pub_count, end=line_end)
+            if args.verbose:
+                try:
+                    for info in node.get_publishers_info_by_topic(topic_name):
+                        full_node_name = f"{info.node_namespace.rstrip('/')}/{info.node_name.lstrip('/')}"
+                        print('Node name: %s' % info.node_name)
+                        print('Node namespace: %s' % info.node_namespace)
+                        print('Topic type: %s' % info.topic_type)
+                        if full_node_name in [info['node_name'] for info in pub_topic_info_rets]:
+                            print('Endpoint type: %s (Agnocast enabled)' % info.endpoint_type.name)
+                        else:
+                            print('Endpoint type: %s' % info.endpoint_type.name)
+                        print('GID: %s' % '.'.join(format(b, '02x') for b in info.endpoint_gid))
+                        print('QoS profile:')
+                        print('  Reliability: %s' % info.qos_profile.reliability.name)
+                        print('  History (Depth): %s (%d)' % (info.qos_profile.history.name, info.qos_profile.depth))
+                        print('  Durability: %s' % info.qos_profile.durability.name)
+                        print('  Lifespan: %s' % info.qos_profile.lifespan)
+                        print('  Deadline: %s' % info.qos_profile.deadline)
+                        print('  Liveliness: %s' % info.qos_profile.liveliness.name)
+                        print('  Liveliness lease duration: %s' % info.qos_profile.liveliness_lease_duration, end=line_end)
+                except NotImplementedError as e:
+                    return str(e)
+
+            agnocast_sub_count = len(sub_topic_info_rets)
+            print('ROS 2 Subscription count: %d' %
+                  node.count_subscribers(topic_name))
+            print('Agnocast Subscription count: %d)' %
+                  agnocast_sub_count, end=line_end)
+            if args.verbose:
+                try:
+                    for info in node.get_subscriptions_info_by_topic(topic_name):
+                        print(info, end=line_end)
+                    for info in sub_topic_info_rets:
+                        nodespace, node_name = self.split_full_node_name(info['node_name'])
+                        print('Node name: %s' % node_name)
+                        print('Node namespace: %s' % nodespace)
+                        print('Topic type: %s' % type_str)
+                        print('Endpoint type: SUBSCROPTION (Agnocast enabled)')
+                        print('QoS profile:')
+                        print('  History (Depth): KEEP_LAST (%d)' % info['qos_depth'])
+                        if info['qos_is_transient_local']:
+                            print('  Durability: TRNSIENT_LOCAL', end=line_end)
+                        else:
+                            print('  Durability: VOLATILE', end=line_end)
+
+                except NotImplementedError as e:
+                    return str(e)
+            ########################################################################

ros2agnocast/setup.py

@@ -14,6 +14,7 @@
     entry_points={
         'ros2topic.verb': [
             'list_agnocast = ros2agnocast.verb.list_agnocast:ListAgnocastVerb',
+            'info_agnocast = ros2agnocast.verb.topic_info_agnocast:TopicInfoAgnocastVerb',
         ],
         'ros2node.verb': [
             'info_agnocast = ros2agnocast.verb.node_info_agnocast:NodeInfoAgnocastVerb',

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)
 

src/agnocast_ioctl_wrapper/src/agnocast_ioctl.hpp

@@ -3,7 +3,10 @@
 #include <cstdint>
 
 #define MAX_TOPIC_NUM 1024
+#define MAX_TOPIC_INFO_RET_NUM 128
+
 #define TOPIC_NAME_BUFFER_SIZE 256
+#define NODE_NAME_BUFFER_SIZE 256
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -20,6 +23,22 @@ 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)

src/agnocast_ioctl_wrapper/src/topic_info.cpp

@@ -0,0 +1,84 @@
+#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;
+
+  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");
+    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;
+  }
+  ////////////////////////////////////////////////////
+
+  *topic_info_ret_count = static_cast<int>(num_nodes);
+  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;
+
+  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");
+    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;
+  }
+  ////////////////////////////////////////////////////
+
+  *topic_info_ret_count = static_cast<int>(num_nodes);
+  return agnocast_topic_info_ret_buffer;
+}
+
+void free_agnocast_topic_info_ret(struct topic_info_ret * array, int count)
+{
+  if (array == nullptr) {
+    return;
+  }
+
+  free(array);
+}
+}