ESP32 CAM not sending any data to ROS2 Jazzy, even though topic is visible and rqt graph is right

[yash17897]

Issue template

• Hardware description: ESP32 CAM
• RTOS: FreeRTOS
• Installation type: Arduino IDE
• Version or commit hash: Jazzy

Steps to reproduce the issue

1. Run these command on your ROS2 terminal:

$ source ~/microros_ws/install/local_setup.sh
$ ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 --buf-size 8192

2. Burn the given Arduino IDE code to the ESP32 CAM module, the camera is OV2640. Put the WiFi credentials also, and put your ROS2 terminal IP address in the code.

#include <micro_ros_arduino.h>
#include <esp_camera.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/compressed_image.h>

#define LED_PIN 13

// Camera Pin Configurations
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 0
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27
#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 21
#define Y4_GPIO_NUM 19
#define Y3_GPIO_NUM 18
#define Y2_GPIO_NUM 5
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22

rcl_publisher_t image_publisher;
sensor_msgs__msg__CompressedImage img_msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define RCCHECK(fn) \
  { \
    rcl_ret_t temp_rc = fn; \
    if ((temp_rc != RCL_RET_OK)) { \
      Serial.printf("[ERROR] RCCHECK failed: %d\n", temp_rc); \
      error_loop(); \
    } \
  }
#define RCSOFTCHECK(fn) \
  { \
    rcl_ret_t temp_rc = fn; \
    if ((temp_rc != RCL_RET_OK)) { \
      Serial.printf("[WARN] RCSOFTCHECK failed: %d\n", temp_rc); \
      return; \
    } \
  }

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

void capture_and_publish_image() {
  Serial.println("[INFO] Capturing image...");
  camera_fb_t *fb = esp_camera_fb_get();
  if (!fb) {
    Serial.println("[ERROR] Camera capture failed!");
    return;
  }
  Serial.printf("[INFO] Captured image of size: %d bytes\n", fb->len);

  if (!rcl_publisher_is_valid(&image_publisher)) {
    Serial.println("[ERROR] Publisher initialization failed! Retrying...");
    rcl_publisher_fini(&image_publisher, &node);
    rclc_publisher_init_default(
      &image_publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, CompressedImage),
      "esp32_cam/image");
  }

  img_msg.format.data = (char *)"jpeg";
  img_msg.format.size = strlen("jpeg");
  img_msg.header.stamp.sec = rmw_uros_epoch_millis() / 1000;
  img_msg.header.stamp.nanosec = (rmw_uros_epoch_millis() % 1000) * 1000000;

  // Use a static buffer to prevent memory fragmentation
  static uint8_t img_buffer[10000];
  if (fb->len > sizeof(img_buffer)) {
    Serial.println("[ERROR] Image too large, skipping...");
    esp_camera_fb_return(fb);
    return;
  }
  memcpy(img_buffer, fb->buf, fb->len);
  img_msg.data.data = img_buffer;
  img_msg.data.size = fb->len;

  Serial.println("[INFO] Image data copied successfully.");

  int max_retries = 3;
  for (int attempt = 1; attempt <= max_retries; attempt++) {
    rcl_ret_t pub_status = rcl_publish(&image_publisher, &img_msg, NULL);
    if (pub_status == RCL_RET_OK) {
      Serial.println("[INFO] Image published successfully.");
      break;
    } else {
      Serial.printf("[ERROR] Image publish failed (Attempt %d/%d) with error code: %d\n", attempt, max_retries, pub_status);
      delay(500);
    }
  }

  esp_camera_fb_return(fb);
  Serial.println("[INFO] Image memory released.");
}



void capture_and_publish_image() {
  Serial.println("[INFO] Capturing image...");
  camera_fb_t * fb = esp_camera_fb_get();

  if (!fb) {
    Serial.println("[ERROR] Camera capture failed!");
    return;
  }

  if (fb->len == 0) {
    Serial.println("[ERROR] Captured image has size 0! Skipping...");
    esp_camera_fb_return(fb);
    return;
  }

  Serial.printf("[INFO] Captured image of size: %d bytes\n", fb->len);

  img_msg.format.data = (char *)"jpeg";
  img_msg.format.size = 4;

  img_msg.data.data = (uint8_t*)malloc(fb->len);
  if (img_msg.data.data == NULL) {
    Serial.println("[ERROR] Memory allocation failed!");
    esp_camera_fb_return(fb);
    return;
  }

  memcpy(img_msg.data.data, fb->buf, fb->len);
  img_msg.data.size = fb->len;
  Serial.println("[INFO] Image data copied successfully.");

  rcl_ret_t ret = rcl_publish(&image_publisher, &img_msg, NULL);
  if (ret == RCL_RET_OK) {
    Serial.println("[INFO] Image published successfully.");
  } else {
    Serial.printf("[ERROR] Image publish failed with error code: %d\n", ret);
  }

  free(img_msg.data.data);
  esp_camera_fb_return(fb);
  Serial.println("[INFO] Image memory released.");
}

void setup() {
  Serial.begin(115200);
  Serial.println("[INFO] Setting up WiFi transport...");
  set_microros_wifi_transports("WiFI_SSID", "WiFi_Password", "xx.xx.xx.xx_ROS2_IP", 8888);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  delay(2000);

  Serial.println("[INFO] Configuring camera...");
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_sccb_sda = SIOD_GPIO_NUM;
  config.pin_sccb_scl = SIOC_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  config.frame_size = FRAMESIZE_QVGA;
  config.jpeg_quality = 20;
  config.fb_count = 1;

  if (esp_camera_init(&config) != ESP_OK) {
    Serial.println("[ERROR] Camera init failed!");
    error_loop();
  }
  Serial.println("[INFO] Camera initialized successfully.");

  allocator = rcl_get_default_allocator();

  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_wifi_node", "", &support));

  if (!rcl_publisher_is_valid(&image_publisher)) {
    Serial.println("[ERROR] Publisher is invalid! Retrying setup...");
    rcl_publisher_fini(&image_publisher, &node);
    rclc_publisher_init_best_effort(
      &image_publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, CompressedImage),
      "esp32_cam/image");
  }

  Serial.println("[INFO] ESP32-CAM setup complete.");
}

void loop() {
  Serial.println("[INFO] Loop start.");
  capture_and_publish_image();
  Serial.println("[INFO] Loop end. Waiting before next capture...");
  delay(10000);
}

Expected behavior

The topics data shall be visible under this command:
$ ros2 topic echo /esp32_cam/image

Actual behavior

The output is blank from above command

Additional information

[4ntn]

Hello, @yash17897,

One thing you can do to start debugging is run the Agent with the argument -v 6. This level of verbosity will show you the data received from the micro-ROS client, and that way you can check if the data is arriving to the agent or not.