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

…essage transmit priority have been added
bakul14 · Aug 4, 2024 · ed7890a · ed7890a
1 parent a1e705c
commit ed7890a
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
@@ -108,7 +108,11 @@ class Node
   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);
   template <typename T_REQ, typename T_RSP, typename OnResponseCb>
+  ServiceClient<T_REQ> create_service_client(CanardMicrosecond const tx_timeout_usec, OnResponseCb&& on_response_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(CanardPortID const response_port_id, CanardMicrosecond const tx_timeout_usec, OnResponseCb&& on_response_cb, 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(CanardPortID const response_port_id, CanardMicrosecond const tx_timeout_usec, OnResponseCb&& on_response_cb, CanardPriority const tx_priority, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC);
 
 #if !defined(__GNUC__) || (__GNUC__ >= 11)
   Registry create_registry();

diff --git a/src/Node.ipp b/src/Node.ipp
@@ -186,7 +186,16 @@ ServiceClient<T_REQ> Node::create_service_client(CanardMicrosecond const tx_time
   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_client<T_REQ, T_RSP>(T_RSP::_traits_::FixedPortId, tx_timeout_usec, on_response_cb, tid_timeout_usec);
+  return create_service_client<T_REQ, T_RSP>(T_RSP::_traits_::FixedPortId, tx_timeout_usec, on_response_cb, CanardPriorityNominal, tid_timeout_usec);
+}
+
+template <typename T_REQ, typename T_RSP, typename OnResponseCb>
+ServiceClient<T_REQ> Node::create_service_client(CanardMicrosecond const tx_timeout_usec, OnResponseCb&& on_response_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_client<T_REQ, T_RSP>(T_RSP::_traits_::FixedPortId, tx_timeout_usec, on_response_cb, tx_priority, tid_timeout_usec);
 }
 
 template <typename T_REQ, typename T_RSP, typename OnResponseCb>
@@ -202,6 +211,36 @@ ServiceClient<T_REQ> Node::create_service_client(CanardPortID const response_por
     *this,
     response_port_id,
     tx_timeout_usec,
+    CanardPriorityNominal,
+    std::forward<OnResponseCb>(on_response_cb)
+  );
+
+  int8_t const rc = canardRxSubscribe(&_canard_hdl,
+                                      CanardTransferKindResponse,
+                                      response_port_id,
+                                      T_RSP::_traits_::ExtentBytes,
+                                      tid_timeout_usec,
+                                      &(clt->canard_rx_subscription()));
+  if (rc < 0)
+    return nullptr;
+
+  return clt;
+}
+
+template <typename T_REQ, typename T_RSP, typename OnResponseCb>
+ServiceClient<T_REQ> Node::create_service_client(CanardPortID const response_port_id, CanardMicrosecond const tx_timeout_usec, OnResponseCb&& on_response_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_client(response_port_id);
+
+  auto clt = std::make_shared<impl::ServiceClient<T_REQ, T_RSP, OnResponseCb>>(
+    *this,
+    response_port_id,
+    tx_timeout_usec,
+    tx_priority,
     std::forward<OnResponseCb>(on_response_cb)
   );
 

diff --git a/src/ServiceClient.hpp b/src/ServiceClient.hpp
@@ -33,10 +33,11 @@ template<typename T_REQ, typename T_RSP, typename OnResponseCb>
 class ServiceClient final : public ServiceClientBase<T_REQ>
 {
 public:
-  ServiceClient(Node & node_hdl, CanardPortID const response_port_id, CanardMicrosecond const tx_timeout_usec, OnResponseCb on_response_cb)
+  ServiceClient(Node & node_hdl, CanardPortID const response_port_id, CanardMicrosecond const tx_timeout_usec, CanardPriority tx_priority, OnResponseCb on_response_cb)
   : _node_hdl{node_hdl}
   , _response_port_id{response_port_id}
   , _tx_timeout_usec{tx_timeout_usec}
+  , _tx_priority{tx_priority}
   , _on_response_cb{on_response_cb}
   , _transfer_id{0}
   { }
@@ -51,6 +52,7 @@ class ServiceClient final : public ServiceClientBase<T_REQ>
   Node & _node_hdl;
   CanardPortID const _response_port_id;
   CanardMicrosecond const _tx_timeout_usec;
+  CanardPriority const _tx_priority;
   OnResponseCb _on_response_cb;
   CanardTransferID _transfer_id;
 };

diff --git a/src/ServiceClient.ipp b/src/ServiceClient.ipp
@@ -45,7 +45,7 @@ bool ServiceClient<T_REQ, T_RSP, OnResponseCb>::request(CanardNodeID const remot
 #pragma GCC diagnostic ignored "-Wpedantic"
   CanardTransferMetadata const transfer_metadata =
     {
-      .priority       = CanardPriorityNominal,
+      .priority       = _tx_priority,
       .transfer_kind  = CanardTransferKindRequest,
       .port_id        = _response_port_id,
       .remote_node_id = remote_node_id,