How to subscribe Joy topic with micro-ROS on ESP32

Question

I'm writing a micro-ROS subscriber for the sensor_msgs/msg/Joy message type, running on an ESP32 board.

I'm trying to display characters on the serial monitor of the Arduino IDE in response to the Dualshock4 input, but nothing is displayed. micro-ROS-Agent is functioning and the joy topic is subscribed.

Is there a solution? Thank you.

The following is the source code written to ESP32.

#include <Arduino.h>
#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/joy.h>

#if !defined(ESP32) && !defined(TARGET_PORTENTA_H7_M7) && !defined(ARDUINO_NANO_RP2040_CONNECT)
#error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect and ESP32 Dev module
#endif

rcl_subscription_t sub_joy;
sensor_msgs__msg__Joy msg_joy;
rclc_executor_t executor;

#define LED_PIN 13
void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void joy_callback(const void *msgin){
  const sensor_msgs__msg__Joy *msg = (const sensor_msgs__msg__Joy *)msgin;

  if (msg->axes.data[6] > 0) {
    Serial.println("Left Pressed.");
  } else if (msg->axes.data[6] < 0) {
    Serial.println("Right Pressed.");
  }

  if (msg->axes.data[7] > 0) {
    Serial.println("Up Pressed.");
  } else if (msg->axes.data[7] < 0) {
    Serial.println("Down Pressed.");
  }

  if (msg->buttons.data[0] == 1) {
    Serial.println("Button X Pressed.");
  }
  
  if (msg->buttons.data[1] == 1) {
    Serial.println("Button O Pressed.");
  }

  if (msg->buttons.data[2] == 1) {
    Serial.println("Button ^ Pressed.");
  }

  if (msg->buttons.data[3] == 1) {
    Serial.println("Button [] Pressed.");
  }

  if (msg->buttons.data[4] == 1) {
    Serial.println("Button L1 Pressed.");
  }

  if (msg->buttons.data[5] == 1) {
    Serial.println("Button R1 Pressed.");
  }

  if (msg->buttons.data[6] == 1) {
    Serial.println("Button L2 Pressed.");
  }

  if (msg->buttons.data[7] == 1) {
    Serial.println("Button R2 Pressed.");
  }
}

void setup(){
  Serial.begin(115200);
  
  set_microros_wifi_transports("SSID", "PASSWORD", "IP", 8888);
  
  pinMode(LED_PIN, OUTPUT);
  delay(2000);

  // Initialize micro-ROS allocator
  rcl_allocator_t allocator = rcl_get_default_allocator();

  // Initialize and modify options
  size_t domain_id = 0;
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_init_options_init(&init_options, allocator);
  rcl_init_options_set_domain_id(&init_options, domain_id);

  // Initialize rclc support object with custom options
  rclc_support_t support;
  rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);

  // Create node object
  rcl_node_t node;
  const char * node_name = "esp32";

  // Init node with configured support object (Node namespace is empty now.)
  rclc_node_init_default(&node, node_name, "", &support);

  // Create subscriber
  const rmw_qos_profile_t * qos_profile_sub = &rmw_qos_profile_sensor_data; // Set Subscriber QoS
  rclc_subscription_init(
    &sub_joy,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy),
    "joy", qos_profile_sub);
  
  // Set executor
  int callback_size = 1;
  executor = rclc_executor_get_zero_initialized_executor();
  rclc_executor_init(&executor, &support.context, callback_size, &allocator);
  rclc_executor_add_subscription(&executor, &sub_joy, &msg_joy, 
                   &joy_callback, ON_NEW_DATA);
  
  Serial.println("setup() Finished.");
}

void loop(){
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
}

Update date: Oct 3rd,2024

After I press the reset button on the ESP32, it shows continuous messages on the agent. However, nothing's changed.