Merge pull request #280 from edu-rossrobotics/feature/service_response_with_metadata

Feature/service response with metadata

Author: aentinger

examples/CAN/OpenCyphal-Service-Client-With-Metadata/OpenCyphal-Service-Client-With-Metadata.ino

@@ -0,0 +1,127 @@
+/*
+ * This example shows how to use the OpenCyphal library to request the performance of a
+ * service from a service server.
+ */
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
+#include <SPI.h>
+
+#include <107-Arduino-Cyphal.h>
+#include <107-Arduino-MCP2515.h>
+#include <107-Arduino-CriticalSection.h>
+
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
+using namespace uavcan::node;
+
+/**************************************************************************************
+ * CONSTANTS
+ **************************************************************************************/
+
+static int const MKRCAN_MCP2515_CS_PIN  = 3;
+static int const MKRCAN_MCP2515_INT_PIN = 7;
+
+/**************************************************************************************
+ * FUNCTION DECLARATION
+ **************************************************************************************/
+
+void onReceiveBufferFull(CanardFrame const &);
+void onExecuteCommand_1_1_Response_Received(ExecuteCommand::Response_1_1 const & rsp, cyphal::TransferMetadata const & metadata);
+
+/**************************************************************************************
+ * GLOBAL VARIABLES
+ **************************************************************************************/
+
+ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },
+                       []() { digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); },
+                       [](uint8_t const data) { return SPI.transfer(data); },
+                       micros,
+                       onReceiveBufferFull,
+                       nullptr);
+
+cyphal::Node::Heap<cyphal::Node::DEFAULT_O1HEAP_SIZE> node_heap;
+cyphal::Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 28);
+
+cyphal::ServiceClient<ExecuteCommand::Request_1_1> srv_client = node_hdl.create_service_client<ExecuteCommand::Request_1_1, ExecuteCommand::Response_1_1>(
+  2*1000*1000UL,
+  onExecuteCommand_1_1_Response_Received);
+
+/**************************************************************************************
+ * SETUP/LOOP
+ **************************************************************************************/
+
+void setup()
+{
+  Serial.begin(9600);
+  while(!Serial) { }
+  delay(1000);
+  Serial.println("|---- OpenCyphal Service Client With Metadata Example ----|");
+
+  /* Setup SPI access */
+  SPI.begin();
+  pinMode(MKRCAN_MCP2515_CS_PIN, OUTPUT);
+  digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH);
+
+  /* Attach interrupt handler to register MCP2515 signaled by taking INT low */
+  pinMode(MKRCAN_MCP2515_INT_PIN, INPUT_PULLUP);
+  attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, LOW);
+
+  /* Initialize MCP2515 */
+  mcp2515.begin();
+  mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ);
+  mcp2515.setNormalMode();
+
+  Serial.println("setup finished");
+}
+
+void loop()
+{
+  /* Process all pending OpenCyphal actions.
+   */
+  {
+    CriticalSection crit_sec;
+    node_hdl.spinSome();
+  }
+
+  /* Publish the request once/second */
+  static unsigned long prev = 0;
+  unsigned long const now = millis();
+  if(now - prev > 1000)
+  {
+    prev = now;
+
+    /* Request some coffee. */
+    Serial.println("Requesting some coffee");
+    std::string const cmd_param("I want a double espresso with cream!");
+    ExecuteCommand::Request_1_1 req;
+    req.command = 0xCAFE;
+    std::copy(cmd_param.begin(), cmd_param.end(), std::back_inserter(req.parameter));
+
+
+    if (!srv_client->request(27 /* remote node id */, req)) {
+      Serial.println("Coffee request failed.");
+    }
+  }
+}
+
+/**************************************************************************************
+ * FUNCTION DEFINITION
+ **************************************************************************************/
+
+void onReceiveBufferFull(CanardFrame const & frame)
+{
+  node_hdl.onCanFrameReceived(frame);
+}
+
+void onExecuteCommand_1_1_Response_Received(ExecuteCommand::Response_1_1 const & rsp, cyphal::TransferMetadata const & metadata)
+{
+  if (rsp.status == ExecuteCommand::Response_1_1::STATUS_SUCCESS)
+    Serial.printf("Coffee successfully retrieved from node %i", metadata.remote_node_id);
+  else
+    Serial.println("I should've suspected trouble when the coffee failed to arrive.");
+}

src/ServiceClient.ipp

@@ -76,7 +76,11 @@ bool ServiceClient<T_REQ, T_RSP, OnResponseCb>::onTransferReceived(CanardRxTrans
   if (!rc) return false;
 
   /* Invoke the user registered callback. */
-  _on_response_cb(rsp);
+  if constexpr (std::is_invocable_v<OnResponseCb, T_RSP, TransferMetadata>) {
+    _on_response_cb(rsp, SubscriptionBase::fillMetadata(transfer));
+  } else {
+    _on_response_cb(rsp);
+  }
 
   return true;
 }