From 18c6ffd5a97e7edca8ee98bee7ced2bfc2ed555a Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 22 Dec 2022 07:47:16 +0100 Subject: [PATCH] Parameterize the entire publisher class over the message type for enhanced type safety. --- examples/OpenCyphal-Blink/OpenCyphal-Blink.ino | 2 +- examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino | 2 +- .../OpenCyphal-Heartbeat-Publisher.ino | 2 +- .../OpenCyphal-ToF-Distance-Sensor-Node.ino | 7 ++++--- src/Node.cpp | 5 ----- src/Node.hpp | 2 +- src/Node.ipp | 6 ++++++ src/Publisher.hpp | 6 ++++-- src/Publisher.ipp | 2 +- 9 files changed, 19 insertions(+), 15 deletions(-) diff --git a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino index 313db39c..0fc290db 100644 --- a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino +++ b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino @@ -64,7 +64,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size()); -Publisher heartbeat_pub = node_hdl.create_publisher(Heartbeat_1_0<>::PORT_ID); +Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID); Heartbeat_1_0<> hb_msg; diff --git a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino index 226bb363..bb3184f5 100644 --- a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino +++ b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino @@ -101,7 +101,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size()); -Publisher heartbeat_pub = node_hdl.create_publisher(uavcan::node::Heartbeat_1_0<>::PORT_ID); +Publisher> heartbeat_pub = node_hdl.create_publisher>(uavcan::node::Heartbeat_1_0<>::PORT_ID); ArduinoNmeaParser nmea_parser(gnss::onRmcUpdate, gnss::onGgaUpdate); diff --git a/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino b/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino index 13394d88..5b96f014 100644 --- a/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino +++ b/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino @@ -41,7 +41,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size()); -Publisher heartbeat_pub = node_hdl.create_publisher(Heartbeat_1_0<>::PORT_ID); +Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID); Heartbeat_1_0<> hb_msg; diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index 6c83df04..915cc08b 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -83,6 +83,8 @@ static SPISettings const MCP2515x_SPI_SETTING{10000000, MSBFIRST, SPI_MODE0}; static CanardNodeID const OPEN_CYPHAL_NODE_ID = 42; static CanardPortID const OPEN_CYPHAL_ID_DISTANCE_DATA = 1001U; +typedef uavcan::primitive::scalar::Real32_1_0 DistanceMessageType; + static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA = { Heartbeat_1_0<>::Mode::INITIALIZATION, @@ -120,8 +122,8 @@ ArduinoMCP2515 mcp2515([]() CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size(), OPEN_CYPHAL_NODE_ID); -Publisher heartbeat_pub = node_hdl.create_publisher(Heartbeat_1_0<>::PORT_ID); -Publisher tof_pub = node_hdl.create_publisher(OPEN_CYPHAL_ID_DISTANCE_DATA); +Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID); +Publisher tof_pub = node_hdl.create_publisher(OPEN_CYPHAL_ID_DISTANCE_DATA); OpenCyphalNodeData node_data = OPEN_CYPHAL_NODE_INITIAL_DATA; OpenCyphalNodeConfiguration node_config = OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION; @@ -296,7 +298,6 @@ void publish_tofDistance(drone::unit::Length const l) { DBG_INFO("[%05lu] Distance = %.3f m", millis(), l.value()); - typedef uavcan::primitive::scalar::Real32_1_0 DistanceMessageType; DistanceMessageType tof_distance_msg; tof_distance_msg.data.value = l.value(); diff --git a/src/Node.cpp b/src/Node.cpp index 1b30c68b..0f3a153d 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -34,11 +34,6 @@ Node::Node(uint8_t * heap_ptr, * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ -Publisher Node::create_publisher(CanardPortID const port_id) -{ - return std::make_shared(_canard_hdl, _canard_tx_queue, port_id); -} - void Node::spinSome(CanFrameTransmitFunc const tx_func) { processRxQueue(); diff --git a/src/Node.hpp b/src/Node.hpp index dad47a98..7869af8e 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -76,7 +76,7 @@ class Node inline void setNodeId(CanardNodeID const node_id) { _canard_hdl.node_id = node_id; } inline CanardNodeID getNodeId() const { return _canard_hdl.node_id; } - Publisher create_publisher(CanardPortID const port_id); + template Publisher create_publisher(CanardPortID const port_id); /* Must be called from the application to process * all received CAN frames. diff --git a/src/Node.ipp b/src/Node.ipp index 0007e70e..7db5af4d 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -9,6 +9,12 @@ * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ +template +Publisher Node::create_publisher(CanardPortID const port_id) +{ + return std::make_shared>(_canard_hdl, _canard_tx_queue, port_id); +} + template bool Node::subscribe(OnTransferReceivedFunc func) { diff --git a/src/Publisher.hpp b/src/Publisher.hpp index 7c248cbe..c7c9ba84 100644 --- a/src/Publisher.hpp +++ b/src/Publisher.hpp @@ -25,6 +25,7 @@ namespace impl * CLASS DECLARATION **************************************************************************************/ +template class Publisher { public: @@ -35,7 +36,7 @@ class Publisher , _transfer_id{0} { } - template bool publish(T const & msg); + bool publish(T const & msg); private: CanardInstance & _canard_hdl; @@ -54,7 +55,8 @@ class Publisher * TYPEDEF **************************************************************************************/ -typedef std::shared_ptr Publisher; +template +using Publisher = std::shared_ptr>; /************************************************************************************** * TEMPLATE IMPLEMENTATION diff --git a/src/Publisher.ipp b/src/Publisher.ipp index 951a4f99..7b2f4205 100644 --- a/src/Publisher.ipp +++ b/src/Publisher.ipp @@ -23,7 +23,7 @@ namespace impl **************************************************************************************/ template -bool Publisher::publish(T const & msg) +bool Publisher::publish(T const & msg) { CanardTransferMetadata const transfer_metadata = {