Skip to content

Commit

Permalink
Merge pull request #280 from edu-rossrobotics/feature/service_respons…
Browse files Browse the repository at this point in the history
…e_with_metadata

Feature/service response with metadata
  • Loading branch information
aentinger authored Aug 18, 2024
2 parents d786624 + 2f330c1 commit ced2f2b
Show file tree
Hide file tree
Showing 2 changed files with 132 additions and 1 deletion.
Original file line number Diff line number Diff line change
@@ -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.");
}
6 changes: 5 additions & 1 deletion src/ServiceClient.ipp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down

0 comments on commit ced2f2b

Please sign in to comment.