micro_ros_arduino: Service call failed

Issue template

• Hardware description: Arduino Portenta H7 on machine control carrier
• RTOS: mbed
• Installation type: micro_ros_arduino + custom messages and services
• Version or commit hash: latest master

Steps to reproduce the issue

Added custom messages to extras/library_generation/extra_packages/ and compiled the static library with:

export ROS_DISTRO=galactic
cd micro_ros_arduino
cp -r ../../ros2_ws/src/emily_uc_messages extras/library_generation/extra_packages/
docker pull microros/micro_ros_static_library_builder:${ROS_DISTRO}
docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:${ROS_DISTRO} -p portenta-m7

Added the static library to the Platform IO IDE project. Cleaned, compiled and downloaded to target.

#include <micro_ros_arduino.h>

#include <Arduino_MachineControl.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/bool.h>

#include <emily_uc_messages/srv/gripper.h>
#include <emily_uc_messages/srv/load_cell.h>
#include <emily_uc_messages/srv/semaphore_lights.h>

rcl_subscription_t subscriber;
std_msgs__msg__Bool msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define LED_PIN 13

// gripper service server object
rcl_service_t gripper_service;
const char * gripper_service_name = "/gripper";
emily_uc_messages__srv__Gripper_Request gripper_request_msg;
emily_uc_messages__srv__Gripper_Response gripper_response_msg;

// load_cell service server object
rcl_service_t load_cell_service;
const char * load_cell_service_name = "/load_cell";
emily_uc_messages__srv__LoadCell_Request load_cell_request_msg;
emily_uc_messages__srv__LoadCell_Response load_cell_response_msg;

// semaphore_lights service server object
rcl_service_t semaphore_lights_service;
const char * semaphore_lights_service_name = "/semaphore_lights";
emily_uc_messages__srv__SemaphoreLights_Request semaphore_lights_request_msg;
emily_uc_messages__srv__SemaphoreLights_Response semaphore_lights_response_msg;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


using namespace machinecontrol;

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void subscription_callback(const void * msgin)
{  
  const std_msgs__msg__Bool * msg = (const std_msgs__msg__Bool *)msgin;

  printf("received: %d\n", msg->data);
  digital_outputs.set(5, (msg->data == 0) ? HIGH : LOW);
}

// gripper service implementation
void gripper_service_callback(const void * request_msg, void * response_msg){
  // Cast messages to expected types
  emily_uc_messages__srv__Gripper_Request * req_in =
    (emily_uc_messages__srv__Gripper_Request *) request_msg;
  emily_uc_messages__srv__Gripper_Response * res_in =
    (emily_uc_messages__srv__Gripper_Response *) response_msg;

  // Handle request message and set the response message values
  printf("gripper received: %d\n", req_in->close_gripper);
  digital_outputs.set(0, req_in->close_gripper ? HIGH : LOW);
  res_in->gripper_closed = req_in->close_gripper;
}

// load_cell service implementation
void load_cell_service_callback(const void * request_msg, void * response_msg){
  // Cast messages to expected types
  emily_uc_messages__srv__LoadCell_Request * req_in =
    (emily_uc_messages__srv__LoadCell_Request *) request_msg;
  emily_uc_messages__srv__LoadCell_Response * res_in =
    (emily_uc_messages__srv__LoadCell_Response *) response_msg;

  // Handle request message and set the response message values
  printf("load_cell received: %d\n", req_in->operation_mode);
  digital_outputs.set(1, req_in->operation_mode ? HIGH : LOW);
  res_in->weight = 0.1f;
  res_in->error_flags = emily_uc_messages__srv__LoadCell_Response__ERROR_FLAGS_NONE;
}

// semaphore_lights service implementation
void semaphore_lights_service_callback(const void * request_msg, void * response_msg){
  // Cast messages to expected types
  emily_uc_messages__srv__SemaphoreLights_Request * req_in =
    (emily_uc_messages__srv__SemaphoreLights_Request *) request_msg;
  emily_uc_messages__srv__SemaphoreLights_Response * res_in =
    (emily_uc_messages__srv__SemaphoreLights_Response *) response_msg;

  // Handle request message and set the response message values
  printf("semaphore_lights GYR received: %d %d %d \n", req_in->green_ctrl, req_in->yellow_ctrl, req_in->red_ctrl);
  digital_outputs.set(2, req_in->green_ctrl == emily_uc_messages__srv__SemaphoreLights_Request__BLINK_NO ? HIGH : LOW);
  digital_outputs.set(3, req_in->yellow_ctrl == emily_uc_messages__srv__SemaphoreLights_Request__BLINK_NO ? HIGH : LOW);
  digital_outputs.set(4, req_in->red_ctrl == emily_uc_messages__srv__SemaphoreLights_Request__BLINK_NO ? HIGH : LOW);
  res_in->green_status = req_in->green_ctrl;
  res_in->yellow_status = req_in->yellow_ctrl;
  res_in->red_status = req_in->red_ctrl;
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH); // turn the LED off after being turned on by pinMode()

  //Set over current behavior of all channels to latch mode:
  digital_outputs.setLatch();

  // Uncomment this line to set over current behavior of all
  // channels to auto retry mode instead of latch mode:
  //digital_outputs.setRetry();
  
  //At startup set all channels to OPEN
  digital_outputs.setAll(0);

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create services
  RCCHECK(rclc_service_init_default(&gripper_service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(emily_uc_messages, srv, Gripper), gripper_service_name));
  RCCHECK(rclc_service_init_default(&load_cell_service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(emily_uc_messages, srv, LoadCell), load_cell_service_name));
  RCCHECK(rclc_service_init_default(&semaphore_lights_service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(emily_uc_messages, srv, SemaphoreLights), semaphore_lights_service_name));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
    "micro_ros_arduino_subscriber"));

  // create executor with support for 3 services and a topic
  RCCHECK(rclc_executor_init(&executor, &support.context, 4, &allocator));

  // Add service callbacks to the executor
  RCCHECK(rclc_executor_add_service(&executor, &gripper_service, &gripper_request_msg, &gripper_response_msg, gripper_service_callback));
  RCCHECK(rclc_executor_add_service(&executor, &load_cell_service, &load_cell_request_msg, &load_cell_response_msg, load_cell_service_callback));
  RCCHECK(rclc_executor_add_service(&executor, &semaphore_lights_service, &semaphore_lights_request_msg, &semaphore_lights_response_msg, semaphore_lights_service_callback));

  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
}

void loop() {
  delay(100);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

Then run the agent on a docker:

docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:galactic serial --dev /dev/ttyACM0 -v6

And after that did:

sudo apt update
sudo apt install ros-galactic-rmw-fastrtps-cpp
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
cd ../ros2_ws/
. /opt/ros/galactic/setup.bash
rm -Rf build/ log/ install/
colcon build --packages-select emily_uc_messages --allow-overriding emily_uc_messages
. install/setup.bash

and got:

# ros2 interface show emily_uc_messages/srv/Gripper 
# coordinator -> µcontroller - gripper service request
bool close_gripper

---

# coordinator -> µcontroller - gripper service response
bool gripper_closed
# ros2 service list
/gripper
# ros2 service call /gripper emily_uc_messages/srv/Gripper close_gripper:\ true\
failed to create client: Type support not from this implementation. Got:
   error not set
   error not set
while fetching it, at /tmp/binarydeb/ros-galactic-rmw-fastrtps-cpp-5.0.1/src/rmw_client.cpp:120, at /tmp/binarydeb/ros-galactic-rcl-3.1.2/src/rcl/client.c:111

Expected behavior

I expected to see three services on the list:

/gripper
/load_cell
/semaphore_lights

But see only one.

And I expected the service call not to fail

Actual behavior

Only one service listed. Service call failed.