Multi-Thread Publisher with custrom transport

[bobkim938]

• Hardware description: ESP32-s3
• Version or commit hash: Humble

I pulled int32_publisher_custom_transport from the examples and replaced micro_ros_task() with the micro_ros_task() given by the multithread_publisher example. So, the code be like:

#include <string.h>
#include <stdio.h>
#include <unistd.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_system.h"
#include "driver/uart.h"

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <rmw_microxrcedds_c/config.h>
#include <rmw_microros/rmw_microros.h>
#include "esp32_serial_transport.h"

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}


rcl_publisher_t publisher_1;
rcl_publisher_t publisher_2;

void thread_1(void * arg)
{
	std_msgs__msg__Int32 msg;
	msg.data = 0;
	while(1){
		RCSOFTCHECK(rcl_publish(&publisher_1, &msg, NULL));
		msg.data++;
		usleep(1000000);
	}
}

void thread_2(void * arg)
{
	std_msgs__msg__Int32 msg;
	msg.data = 0;
	while(1){
		RCSOFTCHECK(rcl_publish(&publisher_2, &msg, NULL));
		msg.data--;
		usleep(500000);
	}
}

void micro_ros_task(void * arg)
{
	rcl_allocator_t allocator = rcl_get_default_allocator();
	rclc_support_t support;

	rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
	RCCHECK(rcl_init_options_init(&init_options, allocator));

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

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

	// create two publishers
	RCCHECK(rclc_publisher_init_default(
		&publisher_1,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"multithread_publisher_1"));

	RCCHECK(rclc_publisher_init_default(
		&publisher_2,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"multithread_publisher_2"));


	xTaskCreate(thread_1,
		"thread_1",
		CONFIG_MICRO_ROS_APP_STACK,
		NULL,
		CONFIG_MICRO_ROS_APP_TASK_PRIO,
		NULL);

	xTaskCreate(thread_2,
		"thread_2",
		CONFIG_MICRO_ROS_APP_STACK,
		NULL,
		CONFIG_MICRO_ROS_APP_TASK_PRIO + 1,
		NULL);

	while(1){
		sleep(100);
	}

	// free resources
	RCCHECK(rcl_publisher_fini(&publisher_1, &node));
	RCCHECK(rcl_publisher_fini(&publisher_2, &node));
	RCCHECK(rcl_node_fini(&node));

  	vTaskDelete(NULL);
}

static size_t uart_port = UART_NUM_0;

void app_main(void)
{
#if defined(RMW_UXRCE_TRANSPORT_CUSTOM)
	rmw_uros_set_custom_transport(
		true,
		(void *) &uart_port,
		esp32_serial_open,
		esp32_serial_close,
		esp32_serial_write,
		esp32_serial_read
	);
#else
#error micro-ROS transports misconfigured
#endif  // RMW_UXRCE_TRANSPORT_CUSTOM

    xTaskCreate(micro_ros_task,
            "uros_task",
            CONFIG_MICRO_ROS_APP_STACK,
            NULL,
            CONFIG_MICRO_ROS_APP_TASK_PRIO,
            NULL);
}

I added "-DUCLIENT_PROFILE_MULTITHREAD=ON", on the colcon.meta and rebuilt the micro-ros components. However, ESP comes up with the following exception handler:

Guru Meditation Error: Core  0 panic'ed (LoadProhibited). Exception was unhandled.
  
  Core  0 register dump:
  PC      : 0x42008849  PS      : 0x00060d30  A0      : 0x8200b647  A1      : 0x3fca2200  
  0x42008849: esp32_serial_open at /home/bob/int32_publisher_custom_transport/main/esp32_serial_transport.c:25
  
  A2      : 0x3fc97ea8  A3      : 0x3fc96b60  A4      : 0x00000000  A5      : 0x3fc96a78  
  A6      : 0x00000004  A7      : 0x00000000  A8      : 0x00000001  A9      : 0x3fca21f0  
  A10     : 0x3fca2200  A11     : 0x3fca2200  A12     : 0x0000001c  A13     : 0x3fca221c  
  A14     : 0x00000004  A15     : 0x00000001  SAR     : 0x0000001d  EXCCAUSE: 0x0000001c  
  EXCVADDR: 0x00000000  LBEG    : 0x400570e8  LEND    : 0x400570f3  LCOUNT  : 0x00000000  
  0x400570e8: memset in ROM
  0x400570f3: memset in ROM
  
  
  
  Backtrace: 0x42008846:0x3fca2200 0x4200b644:0x3fca2240 0x42009b03:0x3fca2260 0x420091ac:0x3fca2280 0x4200e884:0x3fca22b0 0x42008cba:0x3fca2370 0x4200872c:0x3fca2390 0x4037baad:0x3fca2420
  0x42008846: esp32_serial_open at /home/bob/int32_publisher_custom_transport/main/esp32_serial_transport.c:17
  0x4200b644: uxr_init_custom_transport at ??:?
  0x42009b03: rmw_uxrce_transport_init at ??:?
  0x420091ac: rmw_init at ??:?
  0x4200e884: rcl_init at ??:?
  0x42008cba: rclc_support_init_with_options at ??:?
  0x4200872c: micro_ros_task at /home/bob/int32_publisher_custom_transport/main/main.c:59
  0x4037baad: vPortTaskWrapper at /home/bob/esp/v5.2.2/esp-idf/components/freertos/FreeRTOS-Kernel/portable/xtensa/port.c:134
  
  
  ELF file SHA256: ff38db6d6
  
  Rebooting...
  ���ESP-ROM:esp32s3-20210327
  Build:Mar 27 2021
  rst:0xc (RTC_SW_CPU_RST),boot:0x8 (SPI_FAST_FLASH_BOOT)
  Saved PC:0x4037597c

Are multithread flag only for UDP transports?
Thank you.