diff --git a/NOTICE b/NOTICE index 636722cf..090d6257 100644 --- a/NOTICE +++ b/NOTICE @@ -28,3 +28,6 @@ eProsima Robert Bosch GmbH Ralph Lange Raphael Vogelgsang + +Dor Ben Harush + Dor Ben Harush diff --git a/apps/take_picture/README.md b/apps/take_picture/README.md new file mode 100644 index 00000000..2593f26a --- /dev/null +++ b/apps/take_picture/README.md @@ -0,0 +1,36 @@ +# esp32-camera configuration + +* Step 1: create and configure + + The `create_firmware_ws.sh` is the same as esp32. + After the `create_firmware` step you need to add the esp32-camera repository to the esp-idf components directory with the following commands. + ``` + pushd microros_ws/firmware/toolchain/esp-idf/components + + git clone https://github.com/espressif/esp32-camera.git + + popd + ``` + The configuration step is also the same as esp32. +* Step 2: menuconfig +``` + Serial flasher config > Flash size (set to 4M) + + Component config > ESP32-specific > Support for external, SPI-connected RAM` (set to true) + + Component config > Driver configurations > RTCIO configuration (set to TRUE) + + Camera configuration > Camera pins. (BOARD_ESP32CAM_AITHINKER is default) + ``` + Another way to enable the above is with the `sdkconfig.defaults` file by adding these lines: + ``` + CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y + CONFIG_RTCIO_SUPPORT_RTC_GPIO_DESC=y + CONFIG_ESP32_SPIRAM_SUPPORT=y + **PICK ONE OPTION** + CONFIG_BOARD_ESP32CAM_AITHINKER=y **OR** CONFIG_BOARD_WROVER_KIT=y + ``` +* Step 3 : build, flash and run a micro-ros-agent + + +* Step 4 : Run `view_image.py` diff --git a/apps/take_picture/app-colcon.meta b/apps/take_picture/app-colcon.meta new file mode 100644 index 00000000..09731019 --- /dev/null +++ b/apps/take_picture/app-colcon.meta @@ -0,0 +1,21 @@ +{ + "names": { + "rmw_microxrcedds": { + "cmake-args": [ + "-DRMW_UXRCE_MAX_NODES=1", + "-DRMW_UXRCE_MAX_PUBLISHERS=1", + "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0", + "-DRMW_UXRCE_MAX_SERVICES=0", + "-DRMW_UXRCE_MAX_CLIENTS=0", + "-DRMW_UXRCE_MAX_HISTORY=12" + ] + }, + "microxrcedds_client": { + "cmake-args": [ + + "-DUCLIENT_UDP_TRANSPORT_MTU=4096", + + ] + } + } +} diff --git a/apps/take_picture/app.c b/apps/take_picture/app.c new file mode 100644 index 00000000..48b06d4f --- /dev/null +++ b/apps/take_picture/app.c @@ -0,0 +1,182 @@ +/** + * This example takes a picture every 1s. + For more information about esp32-camera - https://github.com/espressif/esp32-camera.git + */ + +// ================================ CODE ====================================== + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + + +#include +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "boards.h" + + +static const char *TAG = "example:take_picture"; + + + +#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; +sensor_msgs__msg__CompressedImage* msg; +camera_fb_t *pic; + + +static camera_config_t camera_config = { + .pin_pwdn = CAM_PIN_PWDN, + .pin_reset = CAM_PIN_RESET, + .pin_xclk = CAM_PIN_XCLK, + .pin_sscb_sda = CAM_PIN_SIOD, + .pin_sscb_scl = CAM_PIN_SIOC, + + .pin_d7 = CAM_PIN_D7, + .pin_d6 = CAM_PIN_D6, + .pin_d5 = CAM_PIN_D5, + .pin_d4 = CAM_PIN_D4, + .pin_d3 = CAM_PIN_D3, + .pin_d2 = CAM_PIN_D2, + .pin_d1 = CAM_PIN_D1, + .pin_d0 = CAM_PIN_D0, + .pin_vsync = CAM_PIN_VSYNC, + .pin_href = CAM_PIN_HREF, + .pin_pclk = CAM_PIN_PCLK, + + //XCLK 20MHz or 10MHz for OV2640 double FPS (Experimental) + .xclk_freq_hz = 20000000, + .ledc_timer = LEDC_TIMER_0, + .ledc_channel = LEDC_CHANNEL_0, + + .pixel_format = PIXFORMAT_JPEG, //YUV422,GRAYSCALE,RGB565,JPEG + .frame_size = FRAMESIZE_HVGA, //QQVGA-UXGA Do not use sizes above QVGA when not JPEG + + .jpeg_quality = 12, //0-63 lower number means higher quality + .fb_count = 1 //if more than one, i2s runs in continuous mode. Use only with JPEG +}; + +void timer_callback(rcl_timer_t * timer, int64_t last_call_time) +{ + UNUSED(last_call_time); + if (timer != NULL) { + + } +} + +static esp_err_t init_camera() +{ + //initialize the camera + esp_err_t err = esp_camera_init(&camera_config); + if (err != ESP_OK) + { + ESP_LOGE(TAG, "Camera Init Failed"); + return err; + } + + return ESP_OK; +} + +void appMain(void * arg) +{ + init_camera(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; + msg = sensor_msgs__msg__CompressedImage__create(); + // create init_options + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + + // create node + rcl_node_t node = rcl_get_zero_initialized_node(); + RCCHECK(rclc_node_init_default(&node, "freertos_picture_publisher", "", &support)); + + // create publisher + RCCHECK(rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, CompressedImage), + "freertos_picture_publisher")); + + // create timer, + rcl_timer_t timer = rcl_get_zero_initialized_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 = rclc_executor_get_zero_initialized_executor(); + RCCHECK(rclc_executor_init(&executor, &support.context, 1, &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_timer(&executor, &timer)); + + char * encoding = "jpeg"; + msg->format.data = (char*)heap_caps_malloc(strlen(encoding) + 1, MALLOC_CAP_8BIT); + msg->format.capacity = strlen(encoding) + 1; + memset(msg->format.data, 0, strlen(encoding) + 1); + memcpy(msg->format.data, encoding, strlen(encoding)); + msg->format.size = strlen(encoding) + 1; + + char* frame_id = "AI_thinker_image"; + msg->header.frame_id.data = (char*)heap_caps_malloc(strlen(frame_id) + 1, MALLOC_CAP_8BIT); + msg->header.frame_id.capacity = strlen(frame_id) + 1; + memset(msg->header.frame_id.data, 0, strlen(frame_id) + 1); + memcpy(msg->header.frame_id.data, frame_id, strlen(frame_id)); + msg->header.frame_id.size = strlen(frame_id) + 1; + + + while(1) + { + rclc_executor_spin_some(&executor, 1000); + usleep(100000); + pic = esp_camera_fb_get(); + //allocate memory for data + msg->data.data = (uint8_t*)heap_caps_malloc(pic->len, MALLOC_CAP_8BIT); + msg->data.capacity = pic->len; + msg->data.size = pic->len; + printf("coopying data...\n"); + if(msg->data.data == NULL) + { + printf("failed to allocate memory. restating in 5 sec.\n"); + vTaskDelay(5000 / portTICK_RATE_MS); + esp_restart(); + } + memcpy(msg->data.data, pic->buf, pic->len); + + printf("pixel format %s \n", msg->format.data); + printf("frame id %s \n", msg->header.frame_id.data); + printf("buffer length %d \n", msg->data.size); + + //publish picture + printf("publisher return value: %d \n", rcl_publish(&publisher, msg, NULL)); + vTaskDelay(200 / portTICK_RATE_MS); + } + + // free resources + RCCHECK(rcl_publisher_fini(&publisher, &node)) + RCCHECK(rcl_node_fini(&node)) + heap_caps_free(msg->format.data); + heap_caps_free(msg->header.frame_id.data); + heap_caps_free(msg->data.data); + sensor_msgs__msg__CompressedImage__destroy(msg); + vTaskDelete(NULL); +} diff --git a/apps/take_picture/boards.h b/apps/take_picture/boards.h new file mode 100644 index 00000000..f8bad850 --- /dev/null +++ b/apps/take_picture/boards.h @@ -0,0 +1,49 @@ +#include "esp_camera.h" + +#define BOARD_WROVER_KIT CONFIG_BOARD_WROVER_KIT +#define BOARD_ESP32CAM_AITHINKER CONFIG_BOARD_ESP32CAM_AITHINKER + +// WROVER-KIT PIN Map +#if BOARD_WROVER_KIT + +#define CAM_PIN_PWDN -1 //power down is not used +#define CAM_PIN_RESET -1 //software reset will be performed +#define CAM_PIN_XCLK 21 +#define CAM_PIN_SIOD 26 +#define CAM_PIN_SIOC 27 + +#define CAM_PIN_D7 35 +#define CAM_PIN_D6 34 +#define CAM_PIN_D5 39 +#define CAM_PIN_D4 36 +#define CAM_PIN_D3 19 +#define CAM_PIN_D2 18 +#define CAM_PIN_D1 5 +#define CAM_PIN_D0 4 +#define CAM_PIN_VSYNC 25 +#define CAM_PIN_HREF 23 +#define CAM_PIN_PCLK 22 + +// ESP32Cam (AiThinker) PIN Map +#elif BOARD_ESP32CAM_AITHINKER + + +#define CAM_PIN_PWDN 32 +#define CAM_PIN_RESET -1 //software reset will be performed +#define CAM_PIN_XCLK 0 +#define CAM_PIN_SIOD 26 +#define CAM_PIN_SIOC 27 + +#define CAM_PIN_D7 35 +#define CAM_PIN_D6 34 +#define CAM_PIN_D5 39 +#define CAM_PIN_D4 36 +#define CAM_PIN_D3 21 +#define CAM_PIN_D2 19 +#define CAM_PIN_D1 18 +#define CAM_PIN_D0 5 +#define CAM_PIN_VSYNC 25 +#define CAM_PIN_HREF 23 +#define CAM_PIN_PCLK 22 + +#endif diff --git a/apps/take_picture/view_images.py b/apps/take_picture/view_images.py new file mode 100755 index 00000000..96787188 --- /dev/null +++ b/apps/take_picture/view_images.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import CompressedImage +import cv2 +import numpy as np + + + + +class MinimalSubscriber(Node): + + def __init__(self, ): + super().__init__('minimal_subscriber') + self.subscription = self.create_subscription(CompressedImage, 'freertos_picture_publisher', self.listener_callback, 10) + self.subscription # prevent unused variable warning + self.bridge = CvBridge() + def listener_callback(self, image_message): + self.get_logger().info('recieved an image') + self.cv_image = self.bridge.compressed_imgmsg_to_cv2(image_message,'8UC3') + #recieve image and co nvert to cv2 image + cv2.imshow('esp32_image', self.cv_image) + cv2.waitKey(3) + + + + +def main(args=None): + rclpy.init(args=args) + minimal_subscriber = MinimalSubscriber() + + rclpy.spin(minimal_subscriber) + minimal_subscriber.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/microros_esp32_extensions/main/Kconfig.projbuild b/microros_esp32_extensions/main/Kconfig.projbuild index 06226d68..9bf1f6f0 100644 --- a/microros_esp32_extensions/main/Kconfig.projbuild +++ b/microros_esp32_extensions/main/Kconfig.projbuild @@ -53,4 +53,86 @@ menu "WiFi Configuration" Set the Maximum retry to avoid station reconnecting to the AP unlimited when the AP is really inexistent. endmenu +endmenu + + + menu "Camera configuration" + + config OV2640_SUPPORT + bool "OV2640 Support" + default y + help + Enable this option if you want to use the OV2640. + Disable this option to save memory. + + config OV7725_SUPPORT + bool "OV7725 Support" + default n + help + Enable this option if you want to use the OV7725. + Disable this option to save memory. + + config OV3660_SUPPORT + bool "OV3660 Support" + default y + help + Enable this option if you want to use the OV3360. + Disable this option to save memory. + + config OV5640_SUPPORT + bool "OV5640 Support" + default y + help + Enable this option if you want to use the OV5640. + Disable this option to save memory. + + config SCCB_HARDWARE_I2C + bool "Use hardware I2C for SCCB" + default y + help + Enable this option if you want to use hardware I2C to control the camera. + Disable this option to use software I2C. + + choice SCCB_HARDWARE_I2C_PORT + bool "I2C peripheral to use for SCCB" + depends on SCCB_HARDWARE_I2C + default SCCB_HARDWARE_I2C_PORT1 + + config SCCB_HARDWARE_I2C_PORT0 + bool "I2C0" + config SCCB_HARDWARE_I2C_PORT1 + bool "I2C1" + + endchoice + + choice CAMERA_TASK_PINNED_TO_CORE + bool "Camera task pinned to core" + default CAMERA_CORE0 + help + Pin the camera handle task to a certain core(0/1). It can also be done automatically choosing NO_AFFINITY. + + config CAMERA_CORE0 + bool "CORE0" + config CAMERA_CORE1 + bool "CORE1" + config CAMERA_NO_AFFINITY + bool "NO_AFFINITY" + + endchoice + menu "Camera Pins" + choice CAMERA_MODEL + bool "Select Camera Pinout" + default BOARD_ESP32CAM_AITHINKER + help + Select Camera Pinout. + + config BOARD_WROVER_KIT + + bool "WROVER-KIT OV2640 Module" + config BOARD_ESP32CAM_AITHINKER + + bool "ESP32-CAM by AI-Thinker" + endchoice + endmenu + endmenu diff --git a/microros_esp32_extensions/sdkconfig.defaults b/microros_esp32_extensions/sdkconfig.defaults index 11698207..04607862 100644 --- a/microros_esp32_extensions/sdkconfig.defaults +++ b/microros_esp32_extensions/sdkconfig.defaults @@ -1,3 +1,4 @@ CONFIG_ESP_MAIN_TASK_STACK_SIZE=3000 CONFIG_FREERTOS_UNICORE=y CONFIG_ESP_TASK_WDT=n +