From 81f0da0bd3dccf1ee43d92f4abab45cee884f07d Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 27 Mar 2023 08:03:37 +0200 Subject: [PATCH 1/5] Feature: Periodically publish information on node publish/subscription/service status. --- .../host-example-01-opencyphal-basic-node.cpp | 2 + src/Node.cpp | 16 ++++ src/Node.hpp | 8 ++ src/Node.ipp | 6 ++ src/util/port/PortListPublisher.hpp | 83 +++++++++++++++++++ src/util/port/PortListPublisherBase.hpp | 51 ++++++++++++ 6 files changed, 166 insertions(+) create mode 100644 src/util/port/PortListPublisher.hpp create mode 100644 src/util/port/PortListPublisherBase.hpp diff --git a/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp b/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp index 68fb2e6c..6d4fc84f 100644 --- a/examples/host-example-01-opencyphal-basic-node/host-example-01-opencyphal-basic-node.cpp +++ b/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 heartbeat_pub = node_hdl.create_publisher (1*1000*1000UL /* = 1 sec in usecs. */); diff --git a/src/Node.cpp b/src/Node.cpp index dadb9141..f45f557c 100644 --- a/src/Node.cpp +++ b/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(new CircularBufferCan(rx_queue_capacity)) : static_cast(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(_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(*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) diff --git a/src/Node.hpp b/src/Node.hpp index 2941eb3f..f04e9ca7 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -18,6 +18,7 @@ #undef min #include #include +#include #include #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 Publisher create_publisher(CanardMicrosecond const tx_timeout_usec); template @@ -134,11 +139,14 @@ class Node std::shared_ptr _canard_rx_queue; size_t const _mtu_bytes; + std::optional _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 void processRxFrame(CanRxQueueItem const * const rx_queue_item); }; diff --git a/src/Node.ipp b/src/Node.ipp index b706faff..980c9bc3 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -30,6 +30,9 @@ Publisher 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>( *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>( *this, port_id, diff --git a/src/util/port/PortListPublisher.hpp b/src/util/port/PortListPublisher.hpp new file mode 100644 index 00000000..84cce1b2 --- /dev/null +++ b/src/util/port/PortListPublisher.hpp @@ -0,0 +1,83 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020-2023 LXRobotics. + * Author: Alexander Entinger + * 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(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(); + } + 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); + } + +private: + ::Publisher _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 */ diff --git a/src/util/port/PortListPublisherBase.hpp b/src/util/port/PortListPublisherBase.hpp new file mode 100644 index 00000000..64c44611 --- /dev/null +++ b/src/util/port/PortListPublisherBase.hpp @@ -0,0 +1,51 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020-2023 LXRobotics. + * Author: Alexander Entinger + * 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 + +#include + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +namespace impl +{ + +/************************************************************************************** + * CLASS DECLARATION + **************************************************************************************/ + +class PortListPublisherBase +{ +public: + virtual ~PortListPublisherBase() { } + virtual void update() = 0; + virtual void add_publisher(CanardPortID const port_id) = 0; + virtual void add_subscriber(CanardPortID const port_id) = 0; +}; + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +} /* impl */ + +/************************************************************************************** + * TYPEDEF + **************************************************************************************/ + +using PortListPublisher = std::shared_ptr; + +#endif /* INC_107_ARDUINO_CYPHAL_LIST_BASE_HPP */ From ef442f0cdcf02e7de50205091a45e616704fd3c2 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 27 Mar 2023 14:17:27 +0200 Subject: [PATCH 2/5] Fix: Major Bug: Use only the encoded amount of bytes, not all bytes. --- src/Publisher.ipp | 4 ++-- src/ServiceClient.ipp | 8 ++++---- src/ServiceServer.ipp | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/Publisher.ipp b/src/Publisher.ipp index 77871cc5..7de94e4f 100644 --- a/src/Publisher.ipp +++ b/src/Publisher.ipp @@ -50,8 +50,8 @@ bool Publisher::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()); } /************************************************************************************** diff --git a/src/ServiceClient.ipp b/src/ServiceClient.ipp index 7af7010e..f28c0d71 100644 --- a/src/ServiceClient.ipp +++ b/src/ServiceClient.ipp @@ -53,14 +53,14 @@ bool ServiceClient::request(CanardNodeID const remot /* Serialize message into payload buffer. */ std::array 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 diff --git a/src/ServiceServer.ipp b/src/ServiceServer.ipp index fb21ee30..ff671ad5 100644 --- a/src/ServiceServer.ipp +++ b/src/ServiceServer.ipp @@ -67,8 +67,8 @@ bool ServiceServer::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()); } /************************************************************************************** From 48393d18c6cf95df61c7dd85e0d82e113749fdd4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 27 Mar 2023 14:26:03 +0200 Subject: [PATCH 3/5] Support publishing of service server/client port IDs with List. --- src/Node.ipp | 6 ++++++ src/util/port/PortListPublisher.hpp | 10 ++++++++++ src/util/port/PortListPublisherBase.hpp | 2 ++ 3 files changed, 18 insertions(+) diff --git a/src/Node.ipp b/src/Node.ipp index 980c9bc3..da5a60a2 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -88,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>( *this, request_port_id, @@ -122,6 +125,9 @@ ServiceClient 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>( *this, response_port_id, diff --git a/src/util/port/PortListPublisher.hpp b/src/util/port/PortListPublisher.hpp index 84cce1b2..f2c00a0f 100644 --- a/src/util/port/PortListPublisher.hpp +++ b/src/util/port/PortListPublisher.hpp @@ -67,6 +67,16 @@ class PortListPublisher final : public PortListPublisherBase _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 _pub; Node::MicrosFunc const _micros_func; diff --git a/src/util/port/PortListPublisherBase.hpp b/src/util/port/PortListPublisherBase.hpp index 64c44611..6d690779 100644 --- a/src/util/port/PortListPublisherBase.hpp +++ b/src/util/port/PortListPublisherBase.hpp @@ -34,6 +34,8 @@ class PortListPublisherBase 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; }; /************************************************************************************** From 7d1d9bfddb56073387cbb22fcf81d920e48e4d68 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 27 Mar 2023 14:28:14 +0200 Subject: [PATCH 4/5] Rule of 5 and all that. --- src/util/port/PortListPublisherBase.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/util/port/PortListPublisherBase.hpp b/src/util/port/PortListPublisherBase.hpp index 6d690779..b3f600cd 100644 --- a/src/util/port/PortListPublisherBase.hpp +++ b/src/util/port/PortListPublisherBase.hpp @@ -30,7 +30,13 @@ namespace impl 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; From 7f71f416e98ed57a4db108d257d3e265cc3092b7 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 27 Mar 2023 15:00:12 +0200 Subject: [PATCH 5/5] Fix: also add the port id of the List_1_0 message to the list. --- src/util/port/PortListPublisher.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/util/port/PortListPublisher.hpp b/src/util/port/PortListPublisher.hpp index f2c00a0f..1db0c3c0 100644 --- a/src/util/port/PortListPublisher.hpp +++ b/src/util/port/PortListPublisher.hpp @@ -39,6 +39,11 @@ class PortListPublisher final : public PortListPublisherBase { _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() { }