107-systems#276 feat: service server constructors that allow to set m…

…essage transmit priority have been added
bakul14 · Aug 4, 2024 · a1e705c · a1e705c
1 parent 9164437
commit a1e705c
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 3 deletions.
diff --git a/src/Node.hpp b/src/Node.hpp
@@ -99,7 +99,11 @@ class Node
   template <typename T_REQ, typename T_RSP, typename OnRequestCb>
   ServiceServer create_service_server(CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC);
   template <typename T_REQ, typename T_RSP, typename OnRequestCb>
+  ServiceServer create_service_server(CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardPriority const tx_priority, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC);
+  template <typename T_REQ, typename T_RSP, typename OnRequestCb>
   ServiceServer create_service_server(CanardPortID const request_port_id, CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC);
+  template <typename T_REQ, typename T_RSP, typename OnRequestCb>
+  ServiceServer create_service_server(CanardPortID const request_port_id, CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardPriority const tx_priority, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC);
 
   template <typename T_REQ, typename T_RSP, typename OnResponseCb>
   ServiceClient<T_REQ> create_service_client(CanardMicrosecond const tx_timeout_usec, OnResponseCb&& on_response_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC);

diff --git a/src/Node.ipp b/src/Node.ipp
@@ -110,7 +110,16 @@ ServiceServer Node::create_service_server(CanardMicrosecond const tx_timeout_use
   static_assert(T_REQ::_traits_::HasFixedPortID, "T_REQ does not have a fixed port id.");
   static_assert(T_RSP::_traits_::HasFixedPortID, "T_RSP does not have a fixed port id.");
 
-  return create_service_server<T_REQ, T_RSP>(T_REQ::_traits_::FixedPortId, tx_timeout_usec, on_request_cb, tid_timeout_usec);
+  return create_service_server<T_REQ, T_RSP>(T_REQ::_traits_::FixedPortId, tx_timeout_usec, on_request_cb, CanardPriorityNominal, tid_timeout_usec);
+}
+
+template <typename T_REQ, typename T_RSP, typename OnRequestCb>
+ServiceServer Node::create_service_server(CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardPriority const tx_priority, CanardMicrosecond const tid_timeout_usec)
+{
+  static_assert(T_REQ::_traits_::HasFixedPortID, "T_REQ does not have a fixed port id.");
+  static_assert(T_RSP::_traits_::HasFixedPortID, "T_RSP does not have a fixed port id.");
+
+  return create_service_server<T_REQ, T_RSP>(T_REQ::_traits_::FixedPortId, tx_timeout_usec, on_request_cb, tid_timeout_usec, tx_priority);
 }
 
 template <typename T_REQ, typename T_RSP, typename OnRequestCb>
@@ -126,6 +135,36 @@ ServiceServer Node::create_service_server(CanardPortID const request_port_id, Ca
     *this,
     request_port_id,
     tx_timeout_usec,
+    CanardPriorityNominal,
+    std::forward<OnRequestCb>(on_request_cb)
+    );
+
+  int8_t const rc = canardRxSubscribe(&_canard_hdl,
+                                      CanardTransferKindRequest,
+                                      request_port_id,
+                                      T_REQ::_traits_::ExtentBytes,
+                                      tid_timeout_usec,
+                                      &(srv->canard_rx_subscription()));
+  if (rc < 0)
+    return nullptr;
+
+  return srv;
+}
+
+template <typename T_REQ, typename T_RSP, typename OnRequestCb>
+ServiceServer Node::create_service_server(CanardPortID const request_port_id, CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardPriority const tx_priority, CanardMicrosecond const tid_timeout_usec)
+{
+  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,
+    tx_timeout_usec,
+    tx_priority,
     std::forward<OnRequestCb>(on_request_cb)
     );
 

diff --git a/src/ServiceServer.hpp b/src/ServiceServer.hpp
@@ -33,10 +33,11 @@ template<typename T_REQ, typename T_RSP, typename OnRequestCb>
 class ServiceServer final : public ServiceServerBase
 {
 public:
-  ServiceServer(Node & node_hdl, CanardPortID const request_port_id, CanardMicrosecond const tx_timeout_usec, OnRequestCb on_request_cb)
+  ServiceServer(Node & node_hdl, CanardPortID const request_port_id, CanardMicrosecond const tx_timeout_usec, CanardPriority const tx_priority, OnRequestCb on_request_cb)
   : _node_hdl{node_hdl}
   , _request_port_id{request_port_id}
   , _tx_timeout_usec{tx_timeout_usec}
+  , _tx_priority{tx_priority}
   , _on_request_cb{on_request_cb}
   { }
   virtual ~ServiceServer();
@@ -49,6 +50,7 @@ class ServiceServer final : public ServiceServerBase
   Node & _node_hdl;
   CanardPortID const _request_port_id;
   CanardMicrosecond const _tx_timeout_usec;
+  CanardPriority const _tx_priority;
   OnRequestCb _on_request_cb;
 };
 

diff --git a/src/ServiceServer.ipp b/src/ServiceServer.ipp
@@ -65,7 +65,7 @@ bool ServiceServer<T_REQ, T_RSP, OnRequestCb>::onTransferReceived(CanardRxTransf
   /* Enqueue the transfer. */
   CanardTransferMetadata const transfer_metadata =
   {
-    .priority       = CanardPriorityNominal,
+    .priority       = _tx_priority,
     .transfer_kind  = CanardTransferKindResponse,
     .port_id        = transfer.metadata.port_id,
     .remote_node_id = transfer.metadata.remote_node_id,