Service call failed - Githubissues

amilcarlucas commented 2 years ago

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.

Acuadros95 commented 2 years ago

As listed on the Suported boards table, Arduino Portenta H7 uses the colcon.meta file to configure the expected memory use. As you can see here, only 1 service is allowed.

Modify this file with your expected entities and recompile the library. You can check this tutorial and this documentation.

amilcarlucas commented 2 years ago

I do not quite get the difference between servicesand clients Are services -> server services ? Are clients -> client services ?

pablogs9 commented 2 years ago

Are services -> server services ? Are clients -> client services ?

That's it.

amilcarlucas commented 2 years ago

Thanks @pablogs9

I changed the colcon.meta file, recompiled the static library, recompiled the program, uploaded it to the board, restarted the uros-agent, and I still get the same behavior :(

pablogs9 commented 2 years ago

Are you initializing the memory in your custom msgs? Could you share emily_uc_messages/Gripper, emily_uc_messages/LoadCell and emily_uc_messages/SemaphoreLights ?

Also this error:

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

seems to be a lack of type support for your custom service types in the ROS 2 side. Are you building and sourcing correctly those type packages?

pablogs9 commented 2 years ago

Also, please share the agent output using -v6 to check what is happening with the micro-ROS service initialization.

amilcarlucas commented 2 years ago

[1647869912.916073] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1647869914.486839] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x477CB297, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 58 43 5B 38 81 00 FC 01
[1647869914.489792] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x477CB297
[1647869914.489880] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x477CB297, address: 0
[1647869914.489932] info     | Root.cpp           | create_client            | create                 | client_key: 0x58435B38, session_id: 0x81
[1647869914.489960] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x58435B38, address: 0
[1647869914.490077] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x58435B38, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1647869914.490356] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x58435B38, len: 56, data: 
0000: 81 80 00 00 01 07 2E 00 00 0A 00 01 01 03 00 00 1F 00 00 00 00 01 CC CC 17 00 00 00 6D 69 63 72
0020: 6F 5F 72 6F 73 5F 61 72 64 75 69 6E 6F 5F 6E 6F 64 65 00 00 00 00 00 00
[1647869914.495674] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x58435B38, participant_id: 0x000(1)
[1647869914.495831] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x58435B38, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1647869914.495893] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x58435B38, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1647869914.496032] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x58435B38, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1647869914.496516] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x58435B38, len: 192, data: 
0000: 81 80 01 00 01 07 B6 00 00 0B 00 08 08 03 00 00 A8 00 00 00 09 00 00 00 2F 67 72 69 70 70 65 72
0020: 00 00 00 00 2F 00 00 00 65 6D 69 6C 79 5F 75 63 5F 6D 65 73 73 61 67 65 73 3A 3A 73 72 76 3A 3A
0040: 64 64 73 5F 3A 3A 47 72 69 70 70 65 72 5F 52 65 71 75 65 73 74 5F 00 08 30 00 00 00 65 6D 69 6C
0060: 79 5F 75 63 5F 6D 65 73 73 61 67 65 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 47 72 69 70 70 65
0080: 72 5F 52 65 73 70 6F 6E 73 65 5F 00 01 C5 00 24 12 00 00 00 72 71 2F 67 72 69 70 70 65 72 52 65
00A0: 71 75 65 73 74 00 01 08 10 00 00 00 72 72 2F 67 72 69 70 70 65 72 52 65 70 6C 79 00 00 01 00 00
[1647869914.497922] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x58435B38, requester_id: 0x000(7), participant_id: 0x000(1)
[1647869914.498044] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x58435B38, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 08 00 00
[1647869914.498090] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x58435B38, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1647869914.498237] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x58435B38, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80

amilcarlucas commented 2 years ago

On the ROS2 side the services seam to work fine:

# 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 interface show emily_uc_messages/srv/LoadCell
# coordinator -> µcontroller - load_cell service request

# operation mode/request
uint8 OPERATION_MODE_GET_WEIGHT=0
uint8 OPERATION_MODE_CALIBRATION_EMPTY=1
uint8 OPERATION_MODE_CALIBRATION_LOADED=2
uint8 operation_mode

---

# µcontroller -> coordinator - load_cell service response

# weight in [g]
float32 weight

# error/status flags
uint8 ERROR_FLAGS_NONE=0
uint8 ERROR_FLAGS_CALIBRATION=1
uint8 ERROR_FLAGS_OVERTEMPERATURE=2
uint8 error_flags

# ros2 interface show emily_uc_messages/srv/SemaphoreLights 
# coordinator -> µcontroller - semaphore lights service request
uint8 BLINK_OFF=0
uint8 BLINK_NO=1
uint8 BLINK_SLOW=2
uint8 BLINK_FAST=3

# each light can have a different blink status
uint8 green_ctrl
uint8 yellow_ctrl
uint8 red_ctrl

---

# µcontroller -> coordinator - semaphore lights service response
uint8 green_status
uint8 yellow_status
uint8 red_status

pablogs9 commented 2 years ago

It seems that the client is creating only the Gripper service. Have you regenerated correctly the library with slots for 3 services?

Also, where does printf prints in your code? Make sure that it is not using the same port as micro-ROS client uses.

pablogs9 commented 2 years ago

Regarding your types, they do not require any special memory initialization in micro-ROS. So ok at that side.

amilcarlucas commented 2 years ago

looks like I did not clean before building the code. I now get all three services listed. But the error message persists:

# 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

# ros2 service call /load_cell emily_uc_messages/srv/LoadCell operation_mode:\ 0\ 
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

# ros2 service call /semaphore_lights emily_uc_messages/srv/SemaphoreLights green_ctrl:\ 1\
yellow_ctrl:\ 1\
red_ctrl:\ 1\ 
Traceback (most recent call last):
  File "/opt/ros/galactic/bin/ros2", line 11, in <module>
    load_entry_point('ros2cli==0.13.1', 'console_scripts', 'ros2')()
  File "/opt/ros/galactic/lib/python3.8/site-packages/ros2cli/cli.py", line 67, in main
    rc = extension.main(parser=parser, args=args)
  File "/opt/ros/galactic/lib/python3.8/site-packages/ros2service/command/service.py", line 41, in main
    return extension.main(args=args)
  File "/opt/ros/galactic/lib/python3.8/site-packages/ros2service/verb/call.py", line 58, in main
    return requester(
  File "/opt/ros/galactic/lib/python3.8/site-packages/ros2service/verb/call.py", line 80, in requester
    values_dictionary = yaml.safe_load(values)
  File "/usr/lib/python3/dist-packages/yaml/__init__.py", line 162, in safe_load
    return load(stream, SafeLoader)
  File "/usr/lib/python3/dist-packages/yaml/__init__.py", line 114, in load
    return loader.get_single_data()
  File "/usr/lib/python3/dist-packages/yaml/constructor.py", line 49, in get_single_data
    node = self.get_single_node()
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 36, in get_single_node
    document = self.compose_document()
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 55, in compose_document
    node = self.compose_node(None, None)
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 84, in compose_node
    node = self.compose_mapping_node(anchor)
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 127, in compose_mapping_node
    while not self.check_event(MappingEndEvent):
  File "/usr/lib/python3/dist-packages/yaml/parser.py", line 98, in check_event
    self.current_event = self.state()
  File "/usr/lib/python3/dist-packages/yaml/parser.py", line 428, in parse_block_mapping_key
    if self.check_token(KeyToken):
  File "/usr/lib/python3/dist-packages/yaml/scanner.py", line 116, in check_token
    self.fetch_more_tokens()
  File "/usr/lib/python3/dist-packages/yaml/scanner.py", line 223, in fetch_more_tokens
    return self.fetch_value()
  File "/usr/lib/python3/dist-packages/yaml/scanner.py", line 577, in fetch_value
    raise ScannerError(None, None,
yaml.scanner.ScannerError: mapping values are not allowed here
  in "<unicode string>", line 1, column 25:
    green_ctrl: 1yellow_ctrl: 1red_ctrl: 1 
                            ^

amilcarlucas commented 2 years ago

I get no printf output, so I guess none of the services gets triggered

pablogs9 commented 2 years ago

This error is related to your ROS 2 side type build & install...

sudo apt update
sudo apt install ros-galactic-rmw-fastrtps-cpp
---- HERE SOURCE ROS 2 INSTALLATION
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
cd ../ros2_ws/
. /opt/ros/galactic/setup.bash
colcon build --packages-select emily_uc_messages --allow-overriding emily_uc_messages
. install/setup.bash

Try to make a source /opt/ros/galactic/setup.bash after installing ros-galactic-rmw-fastrtps-cpp in order to make sure that the type support for your types is being generated also for Fast DDS

amilcarlucas commented 2 years ago

OK, thanks

pablogs9 commented 2 years ago

Also make sure that you run ros2 service call /gripper emily_uc_messages/srv/Gripper "{close_gripper: true}" in the same terminal.

amilcarlucas commented 2 years ago

Again the same issue, I had to rm -Rf build/ log/ install/ and rebuild messages. Now I get:

# ros2 service call /gripper emily_uc_messages/srv/Gripper "{close_gripper: true}"
requester: making request: emily_uc_messages.srv.Gripper_Request(close_gripper=True)

response:
emily_uc_messages.srv.Gripper_Response(gripper_closed=True)

So the code is working, and I can confirm that the hardware also works! You rock. Thanks

micro-ROS / micro_ros_arduino

Service call failed #861

Issue template

Steps to reproduce the issue

Expected behavior

Actual behavior