rclc node fails to initialize multiple services
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 ...
Added an edit to post, too long for comment here.
Basically traced RCCHECK back to
Anyone kind enough here to provide some insight why this fails? Thank you!
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:
Any thoughts?