micro_ros_arduino: Invoking a service yields 2 issues -- (a) incorrect data is received by the service handler on the MCU, and (b) no response is received by the caller

I’ve looked through the previous issues regarding service invocations, and incorporated the suggestions into my code, but I still face issues with service invocation. I am trying to use a precompiled micro_ros_arduino library in platformio (obtained from the below repository):

https://github.com/DominikN/micro_ros_arduino.git
(branch: galactic_custom_build)

I’m hoping for one of the following: (a) help with what I might be doing wrong in the node.cpp code below, or (b) which other micro_ros_arduino library/repository I should include in platformio.ini that would work on an ESP32 MCU. (The original one at github.com/micro-ROS/micro_ros_arduino.git does not work for the platformio + esp32 + arduino combination).

Thanks in advance for your help!

Issue template

• Hardware description: Ubuntu 20.04 (Agent), ESP32 (Client)
• RTOS: Arduino
• Installation type: Platformio
• Version or commit hash: https://github.com/DominikN/micro_ros_arduino.git#galactic_custom_build

Steps to reproduce the issue

I’ve implemented a node with just one service (“/control”) handler:

node
  /platformio.ini
  /src
    /node.cpp

Platformio.ini file:

[env]
platform = espressif32
framework = arduino

[env:esp32_node]
board = esp32dev
monitor_speed = 115200
upload_speed = 921600
src_filter = +<node.cpp>
lib_deps =
	https://github.com/DominikN/micro_ros_arduino#galactic-custom-build
build_flags = 
    -L $PROJECT_DIR/.pio/libdeps/$PIOENV/micro_ros_arduino/src/esp32_5_2_0/
    -l microros

Node.cpp file:

#include <micro_ros_arduino.h>
#include <micro_ros_utilities/string_utilities.h>
#include <rcl/error_handling.h>
#include <rcl/rcl.h>
#include <rclc/executor.h>
#include <rclc/rclc.h>
#include <std_msgs/msg/string.h>
#include <stdio.h>

#include <test_msgs/srv/basic_types.h>

// ------------------
// Setup wifi
// ------------------
const char *ssid = "........";          // <<<<<< FILL >>>>>>>
const char *password = ".........";     // <<<<<< FILL >>>>>>>
const char* agent_ip = ".........";     // <<<<<< FILL >>>>>>>
const uint agent_port = ".........";    // <<<<<< FILL >>>>>>>

// ----------------------
// Error handling macros
// ----------------------
#define RCCHECK(fn, error_handler)    \
  {                                   \
    rcl_ret_t temp_rc = fn;           \
    if ((temp_rc != RCL_RET_OK)) {    \
      Serial.printf("(!=%d) [line %d] [%s]", temp_rc, __LINE__, rcutils_get_error_string().str); \
      error_handler;                  \
    }                                 \
  }

#define RCCRETRY(fn)                  \
  {                                   \
    rcl_ret_t temp_rc;                \
    do {                              \
      temp_rc = fn;                   \
    } while (temp_rc != RCL_RET_OK);  \
  }

#define RCSOFTCHECK(fn)               \
  {                                   \
    rcl_ret_t temp_rc = fn;           \
    if ((temp_rc != RCL_RET_OK)) {    \
      Serial.printf("(!=%d) [line %d] [%s]", temp_rc, __LINE__, rcutils_get_error_string().str); \
    }                                 \
  }


// --------------------
// PIN initialization
// --------------------
#define LED_PIN 2


// --------------------
// Node initialization
// --------------------
bool uros_initialized = false;
rcl_node_t node;
rclc_support_t support;
rcl_init_options_t init_options;
rcl_allocator_t allocator;
rclc_executor_t executor;
int num_execution_handles = 1; // 1 service handler

// ------------------
// Service objects
// ------------------
rcl_service_t service;
test_msgs__srv__BasicTypes_Request request;
test_msgs__srv__BasicTypes_Response response;

// ------------------
// Teardown
// ------------------
void teardown_ros() {
  if (uros_initialized) {
    rcl_ret_t rc = rcl_service_fini(&service, &node);
    rc += rcl_node_fini(&node);
    rc += rclc_executor_fini(&executor);
    rc += rclc_support_fini(&support);
    test_msgs__srv__BasicTypes_Request__fini(&request);
    test_msgs__srv__BasicTypes_Response__fini(&response);
    if (rc != RCL_RET_OK) {
      Serial.println("Error while cleaning up!");
    }
    uros_initialized = false;
  }
}

// ------------------
// Error loop
// ------------------
void error_loop() {
  teardown_ros();
  while (1) {
    Serial.println("error loop");
    delay(200);
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  }
}

// ------------------
// Control service handler
// ------------------
void service_callback(const void * req_in, void * res_in) {
  Serial.print("<Service Invocation callback>");
  test_msgs__srv__BasicTypes_Request * _request = (test_msgs__srv__BasicTypes_Request *) req_in;
  test_msgs__srv__BasicTypes_Response * _response = (test_msgs__srv__BasicTypes_Response *) res_in;
  _response->int16_value = _request->int16_value - 1;
  Serial.printf("Control/State: %d / %d~\n", (int) _request->int16_value, (int) _response->int16_value);
}

// ------------------
// Initialization
// ------------------
void initialize_ros() {
  Serial.print("Initializing ros");
  set_microros_wifi_transports((char*)ssid, (char*)password, (char*)agent_ip, (uint)agent_port);
  
  // create allocator
  Serial.print(".");
  allocator = rcl_get_default_allocator();

  // create init_options
  Serial.print(".");
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator), error_loop());

  // setup node
  Serial.print(".");
  node = rcl_get_zero_initialized_node();
  Serial.print(".");
  RCCHECK(rclc_node_init_default(&node, "control_node", "arm/forearm", &support), error_loop());

  // setup executor
  Serial.print(".");
  executor = rclc_executor_get_zero_initialized_executor();
  Serial.print(".");
  RCCHECK(rclc_executor_init(&executor, &support.context, num_execution_handles, &allocator), error_loop());
  Serial.print(".");
  RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(2000)), error_loop());

  // setup service
  Serial.print(".");
  test_msgs__srv__BasicTypes_Request__init(&request);
  Serial.print(".");
  test_msgs__srv__BasicTypes_Response__init(&response);
  Serial.print(".");
  RCCHECK(rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes), "/control"), error_loop());
  Serial.print(".");
  RCCHECK(rclc_executor_add_service(&executor, &service, (void*) &request, (void*) &response, &service_callback), error_loop());

  Serial.print(".");

  uros_initialized = true;

  Serial.println("Done");
}

// ------------------
// Responsiveness variables
// ------------------
int loop_frequency = 2;                        // Hz


// ------------------
// Setup()
// ------------------
void setup() {
  // Setup pins
  pinMode(LED_PIN, OUTPUT);
  Serial.begin(115200);

  Serial.println("Initializing:");
  delay(3000);

  initialize_ros();

  digitalWrite(LED_PIN, HIGH);
  Serial.println("Startup complete!");
  Serial.print("[Looping]");
}

// ------------------
// Loop()
// ------------------
void loop() {
  int delay_ms = (1.0 / loop_frequency) * 1000;
  delay(delay_ms);

  Serial.print("\n. ");
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
}

Run the agent with:

ros2 run micro_ros_agent micro_ros_agent udp4 -p 8888 -v 5

Flash the MCU via platformio:

rst:0x1 (POWERON_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:2
load:0x3fff0018,len:4
load:0x3fff001c,len:1044
load:0x40078000,len:10124
load:0x40080400,len:5828
entry 0x400806a8
Initializing:
Initializing ros............Done
Startup complete!
[Looping]
. 
. 
<...>

Issue the service call:

ros2 service call /control test_msgs/srv/BasicTypes "{int16_value: 5}"

Expected behavior

The MCU’s service handler should receive the right values sent by the caller, and should return with the response to the caller.

Actual behavior

The request message received by the service handler doesn’t have the right value set (“int16_value”), and the response is never returned to the caller.

The client seems to have registered appropriately with the agent:

[1644217216.858359] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1644217216.858487] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 5
[1644217224.160057] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24
[1644217224.160361] info     | Root.cpp           | create_client            | create                 | client_key: 0x3AB0877C, session_id: 0x81
[1644217224.160507] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x3AB0877C, address: 192.168.1.44:47138
[1644217224.160709] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 19
[1644217224.171900] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 56
[1644217224.200179] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x3AB0877C, participant_id: 0x000(1)
[1644217224.200383] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 14
[1644217224.200423] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 13
[1644217224.230011] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 13
[1644217224.230437] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 180
[1644217224.231780] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x3AB0877C, requester_id: 0x000(7), participant_id: 0x000(1)
[1644217224.231888] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 14
[1644217224.231945] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 13
[1644217224.238551] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 13
[1644217224.760102] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 24
[1644217224.760188] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 13
[1644217224.760383] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 13
[1644217224.760532] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 13
[1644217224.771841] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x3AB0877C, len: 24
[1644217224.772136] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x3AB0877C, len: 13

But, when I call the ‘/control’ service via the command line, like so:

ros2 service call /control test_msgs/srv/BasicTypes "{int16_value: 5}"

… I get:

1644215962.774168 [0]       ros2: using network interface eno1 (udp/192.168.1.100) selected arbitrarily from: eno1, docker0
requester: making request: test_msgs.srv.BasicTypes_Request(bool_value=False, byte_value=b'\x00', char_value=0, float32_value=0.0, float64_value=0.0, int8_value=0, uint8_value=0, int16_value=5, uint16_value=0, int32_value=0, uint32_value=0, int64_value=0, uint64_value=0, string_value='')

… which never returns.

The service handler on the client gets invoked with the wrong request values (int16_value = 0 instead of 5):

. <Service Invocation callback>Control/State: 0 / -1~
.
.
.
<...>

The loop() continues on the MCU, but the caller never gets a response from the service.

.
.
.
.
<...>

Additional information

I can repeatedly issue the service calls. They are received on the MCU, but the issues above are consistent (missing data in the request message, and no response received by the caller).

. <Service Invocation callback>Control/State: 0 / -1~
.
.
. <Service Invocation callback>Control/State: 0 / -1~
.
.
.
. <Service Invocation callback>Control/State: 0 / -1~