micro-ROS/micro_ros_espidf_component

ESP32 create subscriber for sensor_msgs/msg/Joy did not work

Closed this issue · 2 comments

louyuoops commented

Issue template

  • Hardware description: ESP32
  • RTOS: ESP-IDF + RTOS
  • Installation type: clone to project's components directory
  • Version or commit hash: humble

Steps to reproduce the issue

I ran the Micro-ROS agent on Ubuntu and started the joy_node.
The joy_node is functioning properly, and it can receive messages from other nodes.
However, when I create a subscriber on an ESP32 to subscribe to the /joy topic, I am unable to receive any data. On the other hand, when I create a subscriber of type int32, publish int32 messages from a ROS 2 node on Ubuntu, and restart, the int32 subscriber can successfully subscribe to the data.
I am a bit puzzled by this. Is it necessary to perform some special steps in Micro-ROS to successfully subscribe to messages with complex structures like sensor_msgs/msg/Joy?

#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 <sensor_msgs/msg/joy.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 LOG_TAG "rostag"

#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_subscription_t int_subsctiber;
rcl_subscription_t subscriber;
std_msgs__msg__Int32 recv_int_msg;
sensor_msgs__msg__Joy recv_msg;

void int_subsctiber_cb(const void * msg_int) {
	const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msg_int;
	printf("Received Int32 message: %d\n", (int)msg->data);
}

void subscription_callback(const void * msgin)
{
	const sensor_msgs__msg__Joy * msg = (const sensor_msgs__msg__Joy *)msgin;
	printf("Received Joy message\n");
	// printf("header: \n");
	// printf("---- stamp: %d\n",  (int)  msg->header.stamp.sec);
	// printf("---- frame_id: %s\n",  msg->header.frame_id.data);
	// printf("axes: \n");
	// for (size_t i = 0; i < msg->axes.size; i++)
	// {
	// 	printf("---- axes[%d]: %f\n", (int)i, msg->axes.data[i]);
	// }
	// printf("buttons: \n");
	// for (size_t i = 0; i < msg->buttons.size; i++)
	// {
	// 	printf("---- buttons[%d]: %ld\n", (int)i, msg->buttons.data[i]);
	// }
}

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

	// Create init_options.
	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
	// Setup support structure.
	RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));

	// Create node.
	rcl_node_t node = rcl_get_zero_initialized_node();
	RCCHECK(rclc_node_init_default(&node, "int32_publisher_subscriber_rclc", "", &support));

	RCCHECK(rclc_subscription_init_default(
		&int_subsctiber,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"int32_topic"));

	// Create subscriber.
	rcl_ret_t ret = rclc_subscription_init_best_effort(
		&subscriber,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy),
		"joy");
	if (ret != RCL_RET_OK) {
		ESP_LOGI(LOG_TAG, "rclc_subscription_init_default failed /JOY");
	}


	// Create executor.
	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
	RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
	unsigned int rcl_wait_timeout = 1000;   // in ms
	RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));

	RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &recv_msg, &subscription_callback, ON_NEW_DATA));
	RCCHECK(rclc_executor_add_subscription(&executor, &int_subsctiber, &recv_int_msg, &int_subsctiber_cb, ON_NEW_DATA));

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

	// Free resources.
	RCCHECK(rcl_subscription_fini(&subscriber, &node));
	RCCHECK(rcl_subscription_fini(&int_subsctiber, &node));
	// RCCHECK(rcl_publisher_fini(&publisher, &node));
	RCCHECK(rcl_node_fini(&node));

  	vTaskDelete(NULL);
}

void app_main(void)
{
	ESP_LOGI("ros-test", "Starting micro-ROS task");
#if defined(CONFIG_MICRO_ROS_ESP_NETIF_WLAN) || defined(CONFIG_MICRO_ROS_ESP_NETIF_ENET)
    ESP_ERROR_CHECK(uros_network_interface_initialize());
#endif
	ESP_LOGI(LOG_TAG, "=================================>>>>>>>");

    //pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
    xTaskCreate(micro_ros_task,
            "uros_task",
            CONFIG_MICRO_ROS_APP_STACK,
            NULL,
            CONFIG_MICRO_ROS_APP_TASK_PRIO,
            NULL);
}

Expected behavior

Actual behavior

Additional information

louyuoops commented

Thanks a lot, it's solved after initialize message memory.