Merge pull request #219 from 107-systems/subject-id-list

Add feature to allow publishing of port ID list.

examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp

@@ -58,6 +58,8 @@ int main(int argc, char ** argv)
   Node node_hdl(node_heap.data(), node_heap.size(), micros, [socket_can_fd] (CanardFrame const & frame) { return (socketcanPush(socket_can_fd, &frame, 1000*1000UL) > 0); });
   std::mutex node_mtx;
 
+  PortListPublisher port_list_pub = node_hdl.create_port_list_publisher();
+
   Publisher<uavcan::node::Heartbeat_1_0> heartbeat_pub = node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>
     (1*1000*1000UL /* = 1 sec in usecs. */);
 

src/Node.cpp

@@ -15,6 +15,7 @@
 
 #include "util/nodeinfo/NodeInfo.hpp"
 #include "util/registry/Registry.hpp"
+#include "util/port/PortListPublisher.hpp"
 
 /**************************************************************************************
  * CTOR/DTOR
@@ -35,6 +36,7 @@ Node::Node(uint8_t * heap_ptr,
 , _canard_tx_queue{canardTxInit(tx_queue_capacity, mtu_bytes)}
 , _canard_rx_queue{(mtu_bytes == CANARD_MTU_CAN_CLASSIC) ? static_cast<CircularBufferBase *>(new CircularBufferCan(rx_queue_capacity)) : static_cast<CircularBufferBase *>(new CircularBufferCanFd(rx_queue_capacity))}
 , _mtu_bytes{mtu_bytes}
+, _opt_port_list_pub{std::nullopt}
 {
   _canard_hdl.node_id = node_id;
   _canard_hdl.user_reference = static_cast<void *>(_o1heap_ins);
@@ -44,6 +46,12 @@ Node::Node(uint8_t * heap_ptr,
  * PUBLIC MEMBER FUNCTIONS
  **************************************************************************************/
 
+PortListPublisher Node::create_port_list_publisher()
+{
+  _opt_port_list_pub = std::make_shared<impl::PortListPublisher>(*this, _micros_func);
+  return _opt_port_list_pub.value();
+}
+
 #if __GNUC__ >= 11
 Registry Node::create_registry()
 {
@@ -69,10 +77,18 @@ NodeInfo Node::create_node_info(uint8_t const protocol_major, uint8_t const prot
 
 void Node::spinSome()
 {
+  processPortList();
   processRxQueue();
   processTxQueue();
 }
 
+void Node::processPortList()
+{
+  if (_opt_port_list_pub.has_value())
+    _opt_port_list_pub.value()->update();
+}
+
+
 void Node::onCanFrameReceived(CanardFrame const & frame)
 {
   if (_mtu_bytes == CANARD_MTU_CAN_CLASSIC)

src/Node.hpp

@@ -18,6 +18,7 @@
 #undef min
 #include <array>
 #include <memory>
+#include <optional>
 #include <functional>
 
 #include "PublisherBase.hpp"
@@ -28,6 +29,7 @@
 #include "CanRxQueueItem.hpp"
 #include "util/nodeinfo/NodeInfoBase.hpp"
 #include "util/registry/registry_impl.hpp"
+#include "util/port/PortListPublisherBase.hpp"
 
 #include "libo1heap/o1heap.h"
 #include "libcanard/canard.h"
@@ -74,6 +76,9 @@ class Node
   inline CanardNodeID getNodeId() const { return _canard_hdl.node_id; }
 
 
+  PortListPublisher create_port_list_publisher();
+
+
   template <typename T>
   Publisher<T> create_publisher(CanardMicrosecond const tx_timeout_usec);
   template <typename T>
@@ -134,11 +139,14 @@ class Node
   std::shared_ptr<CircularBufferBase> _canard_rx_queue;
   size_t const _mtu_bytes;
 
+  std::optional<PortListPublisher> _opt_port_list_pub;
+
   static void * o1heap_allocate(CanardInstance * const ins, size_t const amount);
   static void   o1heap_free    (CanardInstance * const ins, void * const pointer);
 
   void processRxQueue();
   void processTxQueue();
+  void processPortList();
   template<size_t MTU_BYTES>
   void processRxFrame(CanRxQueueItem<MTU_BYTES> const * const rx_queue_item);
 };

src/Node.ipp

@@ -30,6 +30,9 @@ Publisher<T> Node::create_publisher(CanardPortID const port_id, CanardMicrosecon
 {
   static_assert(!T::_traits_::IsServiceType, "T is not message type");
 
+  if (_opt_port_list_pub.has_value())
+    _opt_port_list_pub.value()->add_publisher(port_id);
+
   return std::make_shared<impl::Publisher<T>>(
     *this,
     port_id,
@@ -49,6 +52,9 @@ Subscription Node::create_subscription(CanardPortID const port_id, OnReceiveCb&&
 {
   static_assert(!T::_traits_::IsServiceType, "T is not message type");
 
+  if (_opt_port_list_pub.has_value())
+    _opt_port_list_pub.value()->add_subscriber(port_id);
+
   auto sub = std::make_shared<impl::Subscription<T, OnReceiveCb>>(
     *this,
     port_id,
@@ -82,6 +88,9 @@ ServiceServer Node::create_service_server(CanardPortID const request_port_id, Ca
   static_assert(T_REQ::_traits_::IsRequest, "T_REQ is not a request");
   static_assert(T_RSP::_traits_::IsResponse, "T_RSP is not a response");
 
+  if (_opt_port_list_pub.has_value())
+    _opt_port_list_pub.value()->add_service_server(request_port_id);
+
   auto srv = std::make_shared<impl::ServiceServer<T_REQ, T_RSP, OnRequestCb>>(
     *this,
     request_port_id,
@@ -116,6 +125,9 @@ ServiceClient<T_REQ> Node::create_service_client(CanardPortID const response_por
   static_assert(T_REQ::_traits_::IsRequest, "T_REQ is not a request");
   static_assert(T_RSP::_traits_::IsResponse, "T_RSP is not a response");
 
+  if (_opt_port_list_pub.has_value())
+    _opt_port_list_pub.value()->add_service_client(response_port_id);
+
   auto clt = std::make_shared<impl::ServiceClient<T_REQ, T_RSP, OnResponseCb>>(
     *this,
     response_port_id,

src/Publisher.ipp

@@ -50,8 +50,8 @@ bool Publisher<T>::publish(T const & msg)
   /* Serialize transfer into a series of CAN frames */
   return _node_hdl.enqueue_transfer(_tx_timeout_usec,
                                     &transfer_metadata,
-                                    msg_buf_bitspan.size() / 8,
-                                    msg_buf_bitspan.aligned_ptr());
+                                    *rc,
+                                    msg_buf.data());
 }
 
 /**************************************************************************************

src/ServiceClient.ipp

@@ -53,14 +53,14 @@ bool ServiceClient<T_REQ, T_RSP, OnResponseCb>::request(CanardNodeID const remot
   /* Serialize message into payload buffer. */
   std::array<uint8_t, T_REQ::_traits_::SerializationBufferSizeBytes> req_buf{};
   nunavut::support::bitspan req_buf_bitspan{req_buf};
-  auto const rc = serialize(req, req_buf_bitspan);
-  if (!rc) return false;
+  auto const req_rc = serialize(req, req_buf_bitspan);
+  if (!req_rc) return false;
 
   /* Serialize transfer into a series of CAN frames. */
   return _node_hdl.enqueue_transfer(_tx_timeout_usec,
                                     &transfer_metadata,
-                                    req_buf_bitspan.size() / 8,
-                                    req_buf_bitspan.aligned_ptr());
+                                    *req_rc,
+                                    req_buf.data());
 }
 
 template<typename T_REQ, typename T_RSP, typename OnResponseCb>

src/ServiceServer.ipp

@@ -67,8 +67,8 @@ bool ServiceServer<T_REQ, T_RSP, OnRequestCb>::onTransferReceived(CanardRxTransf
   /* Serialize transfer into a series of CAN frames */
   return _node_hdl.enqueue_transfer(_tx_timeout_usec,
                                     &transfer_metadata,
-                                    rsp_buf_bitspan.size() / 8,
-                                    rsp_buf_bitspan.aligned_ptr());
+                                    *rsp_rc,
+                                    rsp_buf.data());
 }
 
 /**************************************************************************************

src/util/port/PortListPublisher.hpp

@@ -0,0 +1,98 @@
+/**
+ * This software is distributed under the terms of the MIT License.
+ * Copyright (c) 2020-2023 LXRobotics.
+ * Author: Alexander Entinger <[email protected]>
+ * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors.
+ */
+
+#ifndef INC_107_ARDUINO_CYPHAL_LIST_HPP
+#define INC_107_ARDUINO_CYPHAL_LIST_HPP
+
+/**************************************************************************************
+ * INCLUDES
+ **************************************************************************************/
+
+#include "PortListPublisherBase.hpp"
+
+#include "../../Node.hpp"
+#include "../../DSDL_Types.h"
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+namespace impl
+{
+
+/**************************************************************************************
+ * CLASS DECLARATION
+ **************************************************************************************/
+
+class PortListPublisher final : public PortListPublisherBase
+{
+public:
+  PortListPublisher(Node & node_hdl, Node::MicrosFunc const micros_func)
+  : _pub{node_hdl.create_publisher<uavcan::node::port::List_1_0>(1*1000*1000UL /* = 1 sec in usecs. */)}
+  , _micros_func{micros_func}
+  , _prev_pub{0}
+  , _list_msg{}
+  {
+    _list_msg.publishers.set_sparse_list();
+    _list_msg.subscribers.set_sparse_list();
+
+    /* This one won't be able automatically, as the object does not exist yet when
+     * this objects publisher is created.
+     */
+    add_publisher(uavcan::node::port::List_1_0::_traits_::FixedPortId);
+  }
+  virtual ~PortListPublisher() { }
+
+  virtual void update() override
+  {
+    static CanardMicrosecond const MAX_PUBLICATION_PERIOD_us = uavcan::node::port::List_1_0::MAX_PUBLICATION_PERIOD * 1000 * 1000UL;
+    auto const now = _micros_func();
+    if ((now - _prev_pub) > MAX_PUBLICATION_PERIOD_us)
+    {
+      _prev_pub = now;
+      _pub->publish(_list_msg);
+    }
+  }
+
+  virtual void add_publisher(CanardPortID const port_id) override
+  {
+    auto sparse_list = _list_msg.publishers.get_sparse_list();
+    sparse_list.push_back(uavcan::node::port::SubjectID_1_0{port_id});
+    _list_msg.publishers.set_sparse_list(sparse_list);
+  }
+
+  virtual void add_subscriber(CanardPortID const port_id) override
+  {
+    auto sparse_list = _list_msg.subscribers.get_sparse_list();
+    sparse_list.push_back(uavcan::node::port::SubjectID_1_0{port_id});
+    _list_msg.subscribers.set_sparse_list(sparse_list);
+  }
+
+  virtual void add_service_server(CanardPortID const request_port_id) override
+  {
+    _list_msg.servers.mask.set(request_port_id);
+  }
+
+  virtual void add_service_client(CanardPortID const response_port_id) override
+  {
+    _list_msg.clients.mask.set(response_port_id);
+  }
+
+private:
+  ::Publisher<uavcan::node::port::List_1_0> _pub;
+  Node::MicrosFunc const _micros_func;
+  CanardMicrosecond _prev_pub;
+  uavcan::node::port::List_1_0 _list_msg;
+};
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+} /* impl */
+
+#endif /* INC_107_ARDUINO_CYPHAL_LIST_HPP */

src/util/port/PortListPublisherBase.hpp

@@ -0,0 +1,59 @@
+/**
+ * This software is distributed under the terms of the MIT License.
+ * Copyright (c) 2020-2023 LXRobotics.
+ * Author: Alexander Entinger <[email protected]>
+ * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors.
+ */
+
+#ifndef INC_107_ARDUINO_CYPHAL_LIST_BASE_HPP
+#define INC_107_ARDUINO_CYPHAL_LIST_BASE_HPP
+
+/**************************************************************************************
+ * INCLUDES
+ **************************************************************************************/
+
+#include <memory>
+
+#include <libcanard/canard.h>
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+namespace impl
+{
+
+/**************************************************************************************
+ * CLASS DECLARATION
+ **************************************************************************************/
+
+class PortListPublisherBase
+{
+public:
+  PortListPublisherBase() = default;
+  virtual ~PortListPublisherBase() { }
+  PortListPublisherBase(PortListPublisherBase const &) = delete;
+  PortListPublisherBase(PortListPublisherBase &&) = delete;
+  PortListPublisherBase &operator=(PortListPublisherBase const &) = delete;
+  PortListPublisherBase &operator=(PortListPublisherBase &&) = delete;
+
+  virtual void update() = 0;
+  virtual void add_publisher(CanardPortID const port_id) = 0;
+  virtual void add_subscriber(CanardPortID const port_id) = 0;
+  virtual void add_service_server(CanardPortID const request_port_id) = 0;
+  virtual void add_service_client(CanardPortID const response_port_id) = 0;
+};
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+} /* impl */
+
+/**************************************************************************************
+ * TYPEDEF
+ **************************************************************************************/
+
+using PortListPublisher = std::shared_ptr<impl::PortListPublisherBase>;
+
+#endif /* INC_107_ARDUINO_CYPHAL_LIST_BASE_HPP */