micro-ROS/micro_ros_espidf_component

Unable to create more than 2 publishers

Eightfingers opened this issue · 2 comments

Eightfingers commented

Issue template

  • Hardware description: esp32-s3
  • RTOS: freeRTOS
  • Version or commit hash: humble with esp-idf 5.1

When I try creating more than 2 publishers, the code will fail at the RCCHECK macro, when creating the 3rd publisher. I am testing on the example int32 publisher by creating more int32 publishers.

Steps to reproduce the issue

Flash a program with more than 2 publishers. The code I used to flash is below.

Expected behavior

Able to create more than 2 publishers.

Actual behavior

RCCHECK macro is triggered and the program aborts.

Additional information

#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 <uros_network_interfaces.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#ifdef CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE
#include <rmw_microros/rmw_microros.h>
#endif

#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;
std_msgs__msg__Int32 msg;

// Additional publishers
rcl_publisher_t publisher2;
std_msgs__msg__Int32 msg2;

rcl_publisher_t publisher3;
std_msgs__msg__Int32 msg3;

rcl_publisher_t publisher4;
std_msgs__msg__Int32 msg4;

rcl_publisher_t publisher5;
std_msgs__msg__Int32 msg5;


void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
	RCLC_UNUSED(last_call_time);
	if (timer != NULL) {
		printf("Publishing: %d\n", (int) msg.data);
		RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
		msg.data++;
	}
}

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));

#ifdef CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE
	rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);

	// Static Agent IP and port can be used instead of autodisvery.
	RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP, CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
	//RCCHECK(rmw_uros_discover_agent(rmw_options));
#endif

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

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

	// create publisher
	RCCHECK(rclc_publisher_init_default(
		&publisher,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"freertos_int32_publisher"));

	// create publisher
	RCCHECK(rclc_publisher_init_default(
		&publisher2,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"freertos_int32_publisher2"));

	// create publisher
	RCCHECK(rclc_publisher_init_default(
		&publisher3,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"freertos_int32_publisher3"));

	// create publisher
	RCCHECK(rclc_publisher_init_default(
		&publisher4,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"freertos_int32_publisher4"));

	// create publisher
	RCCHECK(rclc_publisher_init_default(
		&publisher5,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"freertos_int32_publisher5"));


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

	// create executor
	rclc_executor_t executor;
	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
	RCCHECK(rclc_executor_add_timer(&executor, &timer));

	msg.data = 0;

	while(1){
		rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
		usleep(10000);
	}

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

  	vTaskDelete(NULL);
}

void app_main(void)
{
#if defined(CONFIG_MICRO_ROS_ESP_NETIF_WLAN) || defined(CONFIG_MICRO_ROS_ESP_NETIF_ENET)
    ESP_ERROR_CHECK(uros_network_interface_initialize());
#endif

    //pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
    xTaskCreate(micro_ros_task,
            "uros_task",
            200000, // Added more memory to see if this will help but it still fails
            NULL,
            CONFIG_MICRO_ROS_APP_TASK_PRIO,
            NULL);
}
pablogs9 commented

Hello @Eightfingers check that:

micro_ros_espidf_component/colcon.meta

Line 40 in 7933234

"-DRMW_UXRCE_MAX_PUBLISHERS=2",

Eightfingers commented

Hello @Eightfingers check that:

micro_ros_espidf_component/colcon.meta

Line 40 in 7933234

"-DRMW_UXRCE_MAX_PUBLISHERS=2",

After editing ^ and running idf.py clean then rebuilding the project, this worked! Thanks!

  • 🎉1