ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

rclc node fails to initialize multiple services

asked 2022-10-20 18:32:21 -0500

PointCloud gravatar image

updated 2022-10-21 00:02:28 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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!

PointCloud gravatar image PointCloud  ( 2022-10-21 00:04:31 -0500 )edit

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?

PointCloud gravatar image PointCloud  ( 2022-10-21 01:54:30 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-10-21 03:47:00 -0500

PointCloud gravatar image

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_ro...

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.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-10-20 18:32:21 -0500

Seen: 161 times

Last updated: Oct 21 '22