Skip to content

Commit

Permalink
Parameterize the entire publisher class over the message type for enh…
Browse files Browse the repository at this point in the history
…anced type safety.
  • Loading branch information
aentinger committed Dec 22, 2022
1 parent 7ddae95 commit 18c6ffd
Show file tree
Hide file tree
Showing 9 changed files with 19 additions and 15 deletions.
2 changes: 1 addition & 1 deletion examples/OpenCyphal-Blink/OpenCyphal-Blink.ino
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },

CyphalHeap<Node::DEFAULT_O1HEAP_SIZE> 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_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(Heartbeat_1_0<>::PORT_ID);

Heartbeat_1_0<> hb_msg;

Expand Down
2 changes: 1 addition & 1 deletion examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },

CyphalHeap<Node::DEFAULT_O1HEAP_SIZE> 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<uavcan::node::Heartbeat_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(uavcan::node::Heartbeat_1_0<>::PORT_ID);

ArduinoNmeaParser nmea_parser(gnss::onRmcUpdate, gnss::onGgaUpdate);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },

CyphalHeap<Node::DEFAULT_O1HEAP_SIZE> 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_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(Heartbeat_1_0<>::PORT_ID);

Heartbeat_1_0<> hb_msg;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<OPEN_CYPHAL_ID_DISTANCE_DATA> DistanceMessageType;

static OpenCyphalNodeData const OPEN_CYPHAL_NODE_INITIAL_DATA =
{
Heartbeat_1_0<>::Mode::INITIALIZATION,
Expand Down Expand Up @@ -120,8 +122,8 @@ ArduinoMCP2515 mcp2515([]()

CyphalHeap<Node::DEFAULT_O1HEAP_SIZE> 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_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(Heartbeat_1_0<>::PORT_ID);
Publisher<DistanceMessageType> tof_pub = node_hdl.create_publisher<DistanceMessageType>(OPEN_CYPHAL_ID_DISTANCE_DATA);

OpenCyphalNodeData node_data = OPEN_CYPHAL_NODE_INITIAL_DATA;
OpenCyphalNodeConfiguration node_config = OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION;
Expand Down Expand Up @@ -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<OPEN_CYPHAL_ID_DISTANCE_DATA> DistanceMessageType;
DistanceMessageType tof_distance_msg;
tof_distance_msg.data.value = l.value();

Expand Down
5 changes: 0 additions & 5 deletions src/Node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<impl::Publisher>(_canard_hdl, _canard_tx_queue, port_id);
}

void Node::spinSome(CanFrameTransmitFunc const tx_func)
{
processRxQueue();
Expand Down
2 changes: 1 addition & 1 deletion src/Node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename T> Publisher<T> create_publisher(CanardPortID const port_id);

/* Must be called from the application to process
* all received CAN frames.
Expand Down
6 changes: 6 additions & 0 deletions src/Node.ipp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,12 @@
* PUBLIC MEMBER FUNCTIONS
**************************************************************************************/

template <typename T>
Publisher<T> Node::create_publisher(CanardPortID const port_id)
{
return std::make_shared<impl::Publisher<T>>(_canard_hdl, _canard_tx_queue, port_id);
}

template <typename T>
bool Node::subscribe(OnTransferReceivedFunc func)
{
Expand Down
6 changes: 4 additions & 2 deletions src/Publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ namespace impl
* CLASS DECLARATION
**************************************************************************************/

template <typename T>
class Publisher
{
public:
Expand All @@ -35,7 +36,7 @@ class Publisher
, _transfer_id{0}
{ }

template <typename T> bool publish(T const & msg);
bool publish(T const & msg);

private:
CanardInstance & _canard_hdl;
Expand All @@ -54,7 +55,8 @@ class Publisher
* TYPEDEF
**************************************************************************************/

typedef std::shared_ptr<impl::Publisher> Publisher;
template <typename T>
using Publisher = std::shared_ptr<impl::Publisher<T>>;

/**************************************************************************************
* TEMPLATE IMPLEMENTATION
Expand Down
2 changes: 1 addition & 1 deletion src/Publisher.ipp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ namespace impl
**************************************************************************************/

template<typename T>
bool Publisher::publish(T const & msg)
bool Publisher<T>::publish(T const & msg)
{
CanardTransferMetadata const transfer_metadata =
{
Expand Down

0 comments on commit 18c6ffd

Please sign in to comment.