0
$\begingroup$

Rosanswers logo

Hi everyone,

I've gotten myself into another (programming) pickle here and would kindly ask for your expert advise.

Attempting to continue building the examples library, I have successfully created the following examples, which all work subscriber publisher and subscriber service (addTwoInts) publisher, subscriber, service (addTwoInts)

Now I'm attempting to add another service, but for some reason it fails, upon attempting to initialise the second service. Instead of creating the service, it gets stuck inside the error_handler and flashes the LED for ever.

This is the line that fails.

RCCHECK(rclc_service_init_default(&service_setBool, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/setbool"));

Callback, service instance and executor have all been added as it should.

Would you kindly take a look at the source code below and let me know what I am missing. Would appreciate your help.

Thank you!

Complete source code:

#include <Arduino.h>
#include <Wire.h>
#include <micro_ros_platformio.h>
#include <example_interfaces/srv/add_two_ints.h>
#include <example_interfaces/srv/set_bool.h>
#include <stdio.h>
#include <rcl/error_handling.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/int64.h>
#include <std_msgs/msg/bool.h>

#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif

// ============================================================================
// INSTANCES, OBJECTS
// ============================================================================
// publisher and subscriber common
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;
rclc_executor_t executor;
rcl_timer_t timer;
unsigned int num_handles = 4;   // 1 subscriber, 1 publisher, 2 service

// service addTwoInts
rcl_service_t service_addTwoInts;

// service setBool
rcl_service_t service_setBool;

rcl_wait_set_t wait_set;

//publisher
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg_pub;

// subscriber
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg_sub;

example_interfaces__srv__AddTwoInts_Response res;
example_interfaces__srv__AddTwoInts_Request req;

example_interfaces__srv__SetBool_Response res_setBool;
example_interfaces__srv__SetBool_Request req_setBool;


// ============================================================================
// DEFINES
// ============================================================================
// I/O - Definitions
#define LED_PIN 13

// cleartext substitutions
// debug
#ifndef DebugMonitor
#define DebugMonitor Serial6
#endif

#define debug(x) DebugMonitor.print(x)
#define debugln(x) DebugMonitor.println(x)

// ============================================================================
// FUNCTION PROTOTYPES
// ============================================================================
#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)){}}

// Error handle loop
void error_loop() {
  while(1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(500);
  }
}
// --------------------------------------------------------
// timer callback (currently just increments msg_pub.data)
void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg_pub, NULL));
    msg_pub.data++;
  }
}
// --------------------------------------------------------
// subscriber callback
void subscription_callback(const void * msgin) {  
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);  
}
// --------------------------------------------------------
// srv.addTwoInts callback
void service_addTwoInts_callback(const void * req, void * res) {
  example_interfaces__srv__AddTwoInts_Request * req_in = (example_interfaces__srv__AddTwoInts_Request *) req;
  example_interfaces__srv__AddTwoInts_Response * res_in = (example_interfaces__srv__AddTwoInts_Response *) res;

  //printf("Service request value: %d + %d.\n", (int) req_in->a, (int) req_in->b);

  res_in->sum = req_in->a + req_in->b;
}
// --------------------------------------------------------
// srv.setBool callback
void service_setBool_callback(const void * req_setBool, void * res_setBool) {
  bool previousState = digitalRead(LED_PIN);
  example_interfaces__srv__SetBool_Request * req_in = (example_interfaces__srv__SetBool_Request *) req_setBool;
  example_interfaces__srv__SetBool_Response * res_in = (example_interfaces__srv__SetBool_Response *) res_setBool;
  
  digitalWrite(LED_PIN, (req_in->data == 0) ? LOW:HIGH);

  if (previousState != digitalRead(LED_PIN)) {
    res_in->success = true;
  }
  else {
    res_in->success = false;
  }
}

// ============================================================================
// SETUP
// ============================================================================
void setup() {
  set_microros_serial_transports(Serial);

  // Debug
  DebugMonitor.begin(115200);

  // I/O setup
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH); 

  delay(1000); 

  allocator = rcl_get_default_allocator();

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

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

  // create service addTwoInts
  RCCHECK(rclc_service_init_default(&service_addTwoInts, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));

  // create service setBool
  RCCHECK(rclc_service_init_default(&service_setBool, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/setbool"));
    
  // create publisher
  RCCHECK(rclc_publisher_init_default(&publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_publisher"));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_subscriber"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), timer_callback));

  // create executor
  //RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_init(&executor, &support.context, num_handles, &allocator));  
  RCCHECK(rclc_executor_add_service(&executor, &service_addTwoInts, &req, &res, service_addTwoInts_callback));
  RCCHECK(rclc_executor_add_service(&executor, &service_setBool, &req_setBool, &res_setBool, service_setBool_callback));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg_sub, &subscription_callback, ON_NEW_DATA));   
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
  
  msg_pub.data = 0;
}

// ============================================================================
// MAIN LOOP
// ============================================================================
void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); 
}

EDIT: I have added some debug code to get to the bottom of this problem, as per the RCCHECK. See below for details:

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

The output I receive in the debug console is

1

Tracing RCL_RET_OK back to where it is defined, I found the following in file types.h:

/// Success return code.
#define RCL_RET_OK RMW_RET_OK
/// Unspecified error return code.
#define RCL_RET_ERROR RMW_RET_ERROR
/// Timeout occurred return code.
#define RCL_RET_TIMEOUT RMW_RET_TIMEOUT
/// Failed to allocate memory return code.
#define RCL_RET_BAD_ALLOC RMW_RET_BAD_ALLOC
/// Invalid argument return code.
#define RCL_RET_INVALID_ARGUMENT RMW_RET_INVALID_ARGUMENT
/// Unsupported return code.
#define RCL_RET_UNSUPPORTED RMW_RET_UNSUPPORTED

Hovering with the mouse over the individual #defines, the tooltip revealed the "1" seen in my debug output resolves to

RMW_RET_ERROR

with the comment above:

/// Unspecified error return code.

Not really helpful, but with some luck somebody in the community has been across this and is willing to provide some insight.

Much appreciated!

Originally posted by PointCloud on ROS Answers with karma: 120 on 2022-10-20

Post score: 1

Original comments

Comment by PointCloud on 2022-10-21:
Added an edit to post, too long for comment here.

Basically traced RCCHECK back to

/// Unspecified error return code.
RMW_RET_ERROR

Anyone kind enough here to provide some insight why this fails? Thank you!

Comment by PointCloud on 2022-10-21:
Just did a test to verify that both services are able to run. Commenting one or the other out and compiling / downloading the code works without issues. So I'm guessing there is an issue with the way the second service is initialized. Nothing in the source code tells me that these functions need to be unique, or that they would interfere with one another. Here is, how both services are initialized:

RCCHECK(rclc_service_init_default(&service_addTwoInts, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));

RCCHECK(rclc_service_init_default(&service_setBool, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/setbool"));

Any thoughts?

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

Answer to this is adding a custom .meta file. This is done by declaring it in the platfomio.ini as so:

board_microros_user_meta = custom.meta

Here the entire platformio.ini:

[env:teensy41]
platform = teensy
board = teensy41
framework = arduino
board_microros_transport = serial
monitor_port = /dev/ttyUSB0
board_microros_distro = humble

board_microros_user_meta = custom.meta

lib_deps =
    https://github.com/micro-ROS/micro_ros_platformio

The the file needs to be created in the root directory of the project

(<workspace>/custom.meta)

The contents can be copied from an example found in the documentation and should look like the below: {

    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=10",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=2",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

This can be found in the documentation under https://github.com/micro-ROS/micro_ros_platformio#other-configuration

Note: the common.meta file makes general adjustments to the library and shall not be modified by the user. Same applies to the colcon.meta. These should NOT be modified manually. AFAIK.

Will sanitize this combined subscriber, publisher, and 2x service example before creating pull request to main.

Originally posted by PointCloud with karma: 120 on 2022-10-21

This answer was ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.