add camera node for esp32s3

jsk_robot_common/jsk_robot_startup/ino/CameraNode/CameraNode.ino

@@ -0,0 +1,146 @@
+// https://github.com/ros-drivers/rosserial/pull/569
+#define ESP_SERIAL
+#define ROSSERIAL_ARDUINO_BLUETOOTH_BLE // set this if you use rosserial over bluetooth BLE
+
+#include <ros.h>
+#include <sensor_msgs/CompressedImage.h>
+
+ros::NodeHandle_<ArduinoHardware, 25, 25, 512, 131072> nh;
+
+sensor_msgs::CompressedImage image_msg;
+ros::Publisher pub("image_raw/compressed", &image_msg);
+
+#include "esp_camera.h"
+#include "esp_bt_device.h"
+
+#define CAMERA_MODEL_XIAO_ESP32S3 // Has PSRAM
+
+#include "camera_pins.h"
+
+bool camera_sign = false;       // Check camera status
+
+void setup() {
+  nh.initNode();
+  nh.advertise(pub);
+
+  Serial.begin(115200);
+  //while(!Serial);
+  //Serial.setDebugOutput(true);
+  //Serial.println();
+
+  camera_config_t config;
+  config.ledc_channel = LEDC_CHANNEL_0;
+  config.ledc_timer = LEDC_TIMER_0;
+  config.pin_d0 = Y2_GPIO_NUM;
+  config.pin_d1 = Y3_GPIO_NUM;
+  config.pin_d2 = Y4_GPIO_NUM;
+  config.pin_d3 = Y5_GPIO_NUM;
+  config.pin_d4 = Y6_GPIO_NUM;
+  config.pin_d5 = Y7_GPIO_NUM;
+  config.pin_d6 = Y8_GPIO_NUM;
+  config.pin_d7 = Y9_GPIO_NUM;
+  config.pin_xclk = XCLK_GPIO_NUM;
+  config.pin_pclk = PCLK_GPIO_NUM;
+  config.pin_vsync = VSYNC_GPIO_NUM;
+  config.pin_href = HREF_GPIO_NUM;
+  config.pin_sscb_sda = SIOD_GPIO_NUM;
+  config.pin_sscb_scl = SIOC_GPIO_NUM;
+  config.pin_pwdn = PWDN_GPIO_NUM;
+  config.pin_reset = RESET_GPIO_NUM;
+  config.xclk_freq_hz = 20000000;
+  config.frame_size = FRAMESIZE_QVGA;
+  //config.frame_size = FRAMESIZE_UXGA;
+  config.pixel_format = PIXFORMAT_JPEG; // for streaming
+  //config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition
+  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
+  config.fb_location = CAMERA_FB_IN_PSRAM;
+  config.jpeg_quality = 12;
+  config.fb_count = 1;
+
+  // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
+  //                      for larger pre-allocated frame buffer.
+  if(config.pixel_format == PIXFORMAT_JPEG){
+    if(psramFound()){
+      config.jpeg_quality = 10;
+      config.fb_count = 2;
+      config.grab_mode = CAMERA_GRAB_LATEST;
+    } else {
+      // Limit the frame size when PSRAM is not available
+      config.frame_size = FRAMESIZE_SVGA;
+      config.fb_location = CAMERA_FB_IN_DRAM;
+    }
+  } else {
+    // Best option for face detection/recognition
+    config.frame_size = FRAMESIZE_240X240;
+#if CONFIG_IDF_TARGET_ESP32S3
+    config.fb_count = 2;
+#endif
+  }
+
+  // camera init
+  esp_err_t err = esp_camera_init(&config);
+  if (err != ESP_OK) {
+    Serial.printf("Camera init failed with error 0x%x", err);
+    return;
+  }
+  Serial.println("Camera ready!");
+  camera_sign = true; // Camera initialization check passes
+
+  sensor_t * s = esp_camera_sensor_get();
+  // initial sensors are flipped vertically and colors are a bit saturated
+  if (s->id.PID == OV3660_PID) {
+    s->set_vflip(s, 1); // flip it back
+    s->set_brightness(s, 1); // up the brightness just a bit
+    s->set_saturation(s, -2); // lower the saturation
+  }
+  // drop down frame size for higher initial frame rate
+  if(config.pixel_format == PIXFORMAT_JPEG){
+    s->set_framesize(s, FRAMESIZE_QVGA);
+  }
+
+  pinMode(LED_BUILTIN, OUTPUT);
+
+  Serial.print("Use 'ble-serial -d ");
+  const uint8_t* point = esp_bt_dev_get_address();
+  for (int i = 0; i < 6; i++) {
+    char str[3];
+    sprintf(str, "%02X", (int)point[i]);
+    Serial.print(str);
+    if (i < 5){
+      Serial.print(":");
+    }
+  }
+  Serial.println("' to connect");
+}
+
+int _loop = 0;
+void loop() {
+  if ( _loop++%30 == 0) {
+    digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN));   // blink the led
+  }
+
+  // Obtaining camera images
+  camera_fb_t* fb = esp_camera_fb_get();
+  if (!fb) {
+    Serial.println("Camera capture failed");
+    delay(10000);
+    return;
+  }
+
+  //Serial.print("size of image buffer : ");
+  //Serial.println(fb->len);
+
+  image_msg.header.frame_id = "/camera";
+  image_msg.header.stamp = nh.now();
+  image_msg.format = "jpeg compressed bgr8";
+  image_msg.data_length  = fb->len;
+  image_msg.data = fb->buf;
+  pub.publish( &image_msg );
+
+  // Release image memory
+  esp_camera_fb_return(fb);
+
+  //
+  nh.spinOnce();
+  delay(10);
+}