MicroROS fails to create all publishers and subscribers and fails to initialise all rclc_timers #526
Description
Dear @pablogs9 and microROS developers,
Hello, this is Akash from India.
I'm facing a problem while using the microROS ESP-IDF component.
I've created a .cpp project that publishes to 7 topics and subscribes to 4 more, all serially.
There are no build errors, however, when the serial agent is started on one terminal instance, I've observed that only the first 2 publishers and 2 subscribers are created, the remaining fail with a return code RCL_RET_ERROR. The created publishers are sending data correctly as expected.
I've also increased the number (corresponding to the total pubs and subs) inside rclc_executor_init() function to be 11 (7 pubs 4 subs), but still no luck.
The main app.cpp file has ~500 lines of code, so I've attached a .zip of project file instead of pasting the entire code here:
uvc_chiller_driver_node_idf.zip
[Note: You may build and flash this project on an ESP32, but I've diverted the microROS serial via UART1 (Tx: GPIO_NUM_17, Rx: GPIO_NUM_16) while logger outputs via UART0]
The microROS task looks like this:
void TASK_microROS(void *args)
{
// Galactic: Node with options...
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
RCCHECK(rcl_init_options_init(&init_options, allocator));
RCCHECK(rcl_init_options_set_domain_id(&init_options, ROS_DOMAIN_ID));
// Initialize rclc support object with custom options
rclc_support_t support;
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
// create node
rcl_node_t node;
RCCHECK(rclc_node_init_default(&node, "UVC_Node", "", &support));
// create publishers
printf("ret1:%d\t", rclc_publisher_init_default(&uvc_raw_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "/uvc/uvc_raw"));
printf("ret2:%d\t", rclc_publisher_init_default(&pressure_raw_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "/uvc/pressure_raw"));
printf("ret3:%d\t", rclc_publisher_init_default(&temperature_raw_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "/uvc/temperature_raw"));
printf("ret4:%d\t", rclc_publisher_init_default(&motion_sensor_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt8MultiArray), "/uvc/motion_raw"));
printf("ret5:%d\t", rclc_publisher_init_default(&led_current_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "/uvc/led_current"));
printf("ret6:%d\t", rclc_publisher_init_default(&sensed_voltage_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), "/uvc/sensed_voltage"));
printf("ret7:%d\t", rclc_publisher_init_default(&flow_rate_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), "/uvc/flow_rate"));
printf("created publishers\n");
// create subscribers
printf("ret8:%d\t", rclc_subscription_init_default(&led_control_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt8MultiArray), "/uvc/led_control"));
printf("ret9:%d\t", rclc_subscription_init_default(&compressor_control_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "/uvc/compressor_control"));
printf("ret10:%d\t", rclc_subscription_init_default(&pump_control_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "/uvc/pump_control"));
printf("ret11:%d\t", rclc_subscription_init_default(&fan_control_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "/uvc/fan_control"));
printf("created subscribers\n");
// create and initialize the timers for each publisher
rcl_timer_t uvc_raw_publisher_timer; RCCHECK(rclc_timer_init_default(&uvc_raw_publisher_timer, &support, RCL_MS_TO_NS(500), ROS2_Publisher_Callback_uvc_raw)); // 2 Hz
rcl_timer_t pressure_raw_publisher_timer; RCCHECK(rclc_timer_init_default(&pressure_raw_publisher_timer, &support, RCL_MS_TO_NS(200), ROS2_Publisher_Callback_pressure_raw)); // 5 Hz
rcl_timer_t temperature_raw_publisher_timer; RCCHECK(rclc_timer_init_default(&temperature_raw_publisher_timer, &support, RCL_MS_TO_NS(500), ROS2_Publisher_Callback_temperature_raw)); // 2 Hz
rcl_timer_t flow_rate_publisher_timer; RCCHECK(rclc_timer_init_default(&flow_rate_publisher_timer, &support, RCL_MS_TO_NS(500), ROS2_Publisher_Callback_flow_rate)); // 2 Hz
rcl_timer_t motion_sensor_publisher_timer; RCCHECK(rclc_timer_init_default(&motion_sensor_publisher_timer, &support, RCL_MS_TO_NS(200), ROS2_Publisher_Callback_motion_sensor)); // 5 Hz
rcl_timer_t led_current_publisher_timer; RCCHECK(rclc_timer_init_default(&led_current_publisher_timer, &support, RCL_MS_TO_NS(200), ROS2_Publisher_Callback_led_current)); // 5 Hz
rcl_timer_t sensed_voltage_publisher_timer; RCCHECK(rclc_timer_init_default(&sensed_voltage_publisher_timer, &support, RCL_MS_TO_NS(1000), ROS2_Publisher_Callback_sensed_voltage)); // 1 Hz
// allocate storage space for incoming and outgoing messages (only for arrays)
uvc_raw.data.capacity = 2; uvc_raw.data.data = (float *)calloc(uvc_raw.data.capacity, sizeof(float)); uvc_raw.data.size = uvc_raw.data.capacity;
pressure_raw.data.capacity = 1; pressure_raw.data.data = (float *)calloc(pressure_raw.data.capacity, sizeof(float)); pressure_raw.data.size = pressure_raw.data.capacity;
temperature_raw.data.capacity = 2; temperature_raw.data.data = (float *)calloc(temperature_raw.data.capacity, sizeof(float)); temperature_raw.data.size = temperature_raw.data.capacity;
motion_sensor.data.capacity = 4; motion_sensor.data.data = (uint8_t *)calloc(motion_sensor.data.capacity, sizeof(uint8_t)); motion_sensor.data.size = motion_sensor.data.capacity;
led_current.data.capacity = 4; led_current.data.data = (float *)calloc(led_current.data.capacity, sizeof(float)); led_current.data.size = led_current.data.capacity;
// create executor
rclc_executor_t executor;
RCCHECK(rclc_executor_init(&executor, &support.context, 11, &allocator)); // here there are 11 publishers and subscribers
printf("created executor\n");
RCCHECK(rclc_executor_add_timer(&executor, &uvc_raw_publisher_timer));
RCCHECK(rclc_executor_add_timer(&executor, &pressure_raw_publisher_timer));
RCCHECK(rclc_executor_add_timer(&executor, &temperature_raw_publisher_timer));
RCCHECK(rclc_executor_add_timer(&executor, &motion_sensor_publisher_timer));
RCCHECK(rclc_executor_add_timer(&executor, &led_current_publisher_timer));
RCCHECK(rclc_executor_add_timer(&executor, &sensed_voltage_publisher_timer));
RCCHECK(rclc_executor_add_timer(&executor, &flow_rate_publisher_timer));
printf("added publishers\n");
RCCHECK(rclc_executor_add_subscription(&executor, &led_control_subscriber, &led_control, &ROS2_Subscriber_Callback_led_control, ON_NEW_DATA));
RCCHECK(rclc_executor_add_subscription(&executor, &compressor_control_subscriber, &compressor_control, &ROS2_Subscriber_Callback_compressor_control, ON_NEW_DATA));
RCCHECK(rclc_executor_add_subscription(&executor, &pump_control_subscriber, &pump_control, &ROS2_Subscriber_Callback_pump_control, ON_NEW_DATA));
RCCHECK(rclc_executor_add_subscription(&executor, &fan_control_subscriber, &fan_control, &ROS2_Subscriber_Callback_fan_control, ON_NEW_DATA));
printf("added subscribers\n");
while (1)
{
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));
}
esp_restart();
}Serial agent output:
Please help me!