From c2d581ae1d39f0b23eb472306c021df5cd468f22 Mon Sep 17 00:00:00 2001 From: Samuel Henrique Date: Wed, 2 Oct 2024 10:11:02 -0400 Subject: [PATCH] Add int32_publisher_custom_transport_usbcdc example (#255) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add int32_publisher_custom_transport_usbcdc example This example demonstrates how to enable ROS2 communication via USB CDC and log output on the ESP32-S2. Reviewed-by: Leandro Mendes do Santos Signed-off-by: Samuel Henrique Guimarães Alencar * Add build for int32_publisher_custom_transport_usbcdc example to the CI workflow Signed-off-by: Samuel Henrique Guimarães Alencar * Fix CI condition for int32_publisher_custom_transport_usbcdc example - Updated conditional logic to ensure builds are triggered only when idf_target is either 'esp32s2' or 'esp32s3' and idf_version is 'espressif/idf:release-v5.2'. Signed-off-by: Samuel Henrique Guimarães Alencar --------- Signed-off-by: Samuel Henrique Guimarães Alencar --- .github/workflows/ci.yml | 9 + .../.gitignore | 5 + .../CMakeLists.txt | 7 + .../README.md | 138 +++++++++++++++ .../app-colcon.meta | 9 + .../esp32s2_usbcdc_logging/CMakeLists.txt | 1 + .../esp32s2_usbcdc_logging.c | 46 +++++ .../esp32s2_usbcdc_logging.h | 32 ++++ .../esp32s2_usbcdc_logging/idf_component.yml | 6 + .../esp32s2_usbcdc_transport/CMakeLists.txt | 4 + .../esp32s2_usbcdc_transport.c | 57 ++++++ .../esp32s2_usbcdc_transport.h | 29 +++ .../idf_component.yml | 6 + .../dependencies.lock | 21 +++ .../main/CMakeLists.txt | 1 + .../main/Kconfig.projbuild | 15 ++ .../main/component.mk | 5 + .../main/main.c | 166 ++++++++++++++++++ .../sdkconfig.defaults | 6 + 19 files changed, 563 insertions(+) create mode 100644 examples/int32_publisher_custom_transport_usbcdc/.gitignore create mode 100644 examples/int32_publisher_custom_transport_usbcdc/CMakeLists.txt create mode 100644 examples/int32_publisher_custom_transport_usbcdc/README.md create mode 100644 examples/int32_publisher_custom_transport_usbcdc/app-colcon.meta create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/CMakeLists.txt create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.c create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.h create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/idf_component.yml create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/CMakeLists.txt create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.c create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.h create mode 100644 examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/idf_component.yml create mode 100644 examples/int32_publisher_custom_transport_usbcdc/dependencies.lock create mode 100644 examples/int32_publisher_custom_transport_usbcdc/main/CMakeLists.txt create mode 100644 examples/int32_publisher_custom_transport_usbcdc/main/Kconfig.projbuild create mode 100644 examples/int32_publisher_custom_transport_usbcdc/main/component.mk create mode 100644 examples/int32_publisher_custom_transport_usbcdc/main/main.c create mode 100644 examples/int32_publisher_custom_transport_usbcdc/sdkconfig.defaults diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index eafe3b50..1bee08b3 100755 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -80,6 +80,15 @@ jobs: idf.py set-target ${{ matrix.idf_target }} idf.py build + - name: Build sample - int32_publisher_custom_transport_usbcdc + shell: bash + if: (matrix.idf_target == 'esp32s2' || matrix.idf_target == 'esp32s3') && matrix.idf_version == 'espressif/idf:release-v5.2' + run: | + . $IDF_PATH/export.sh + cd micro_ros_espidf_component/examples/int32_publisher_custom_transport_usbcdc + idf.py set-target ${{ matrix.idf_target }} + idf.py build + - name: Build sample - multithread_publisher shell: bash run: | diff --git a/examples/int32_publisher_custom_transport_usbcdc/.gitignore b/examples/int32_publisher_custom_transport_usbcdc/.gitignore new file mode 100644 index 00000000..e9fcd3f8 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/.gitignore @@ -0,0 +1,5 @@ +build +sdkconfig +sdkconfig.old +managed_components +.vscode \ No newline at end of file diff --git a/examples/int32_publisher_custom_transport_usbcdc/CMakeLists.txt b/examples/int32_publisher_custom_transport_usbcdc/CMakeLists.txt new file mode 100644 index 00000000..b416a561 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.5) + +set (EXTRA_COMPONENT_DIRS "./../../.") + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(int32_publisher) + diff --git a/examples/int32_publisher_custom_transport_usbcdc/README.md b/examples/int32_publisher_custom_transport_usbcdc/README.md new file mode 100644 index 00000000..7536afa5 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/README.md @@ -0,0 +1,138 @@ + +# USB-CDC Custom Transport Example + +| Supported Targets | ESP32-S2 | ESP32-S3 | +|-------------------|----------|----------| + +This example demonstrates how to set up the ESP32-S2/S3 to function as a USB Serial Device (CDC-ACM) and communicate with micro-ROS agent using USB-CDC custom transport. + +The [TinyUSB component](https://components.espressif.com/components/espressif/esp_tinyusb) is used as the USB stack. + +This example is based on the [int32_publisher_custom_transport](https://github.com/micro-ROS/micro_ros_espidf_component/tree/jazzy/examples/int32_publisher_custom_transport), the [TinyUSB Serial Device Example](https://github.com/espressif/esp-idf/tree/master/examples/peripherals/usb/device/tusb_serial_device), and the [TinyUSB Console Example](https://github.com/espressif/esp-idf/tree/master/examples/peripherals/usb/device/tusb_console) for log output. + +## How to use example + +This example is configured to use the two interfaces of USB-CDC. One interface is used for the micro-ROS communication, and the other interface is used for the log output. + +### Hardware Required + +This example can be run on any development board that has a USB-CDC interface. + +### Configure the project + +Set the target device in the project configuration: + +```bash +idf.py set-target esp32s2 # or esp32s3 +``` + +If you want to use only the micro-ROS communication interface, you need to turn off log output in menuconfig. Run `idf.py menuconfig` and navigate to `Component config → Log output → Default log verbosity` and set it to `No output`. You should also set `Component config → TinyUSB Stack → Communication Device Class (CDC) → CDC Channel Count` to 1. + +### Build and Flash + +> [!NOTE] +> The ESP32-S2/S3 chip needs to be in bootloader mode before it can be detected as a DFU device and flash. This can be achieved by pulling GPIO0 down (e.g., pressing the BOOT button), pulling RESET down for a moment, and releasing GPIO0. + +#### Build the project + +Build DFU image: + +```bash +idf.py dfu +``` + +#### Flash the project + +Put the ESP32-S2/S3 into bootloader mode and run the following command: + +```bash +idf.py dfu-flash +``` + +### Run micro-ROS Agent + +```bash +export ROS_DOMAIN_ID=100 # Set the ROS2 domain ID +ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 +``` + +Output expected: + +```bash +[1724443525.673894] info | TermiosAgentLinux.cpp | init | running... | fd: 3 +[1724443525.674071] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 4 +[1724443529.936542] info | TermiosAgentLinux.cpp | init | running... | fd: 3 +[1724443531.062646] info | Root.cpp | create_client | create | client_key: 0x3E801A05, session_id: 0x81 +[1724443531.062805] info | SessionManager.hpp | establish_session | session established | client_key: 0x3E801A05, address: 0 +[1724443531.107532] info | ProxyClient.cpp | create_participant | participant created | client_key: 0x3E801A05, participant_id: 0x000(1) +[1724443531.137064] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x3E801A05, topic_id: 0x000(2), participant_id: 0x000(1) +[1724443531.167351] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x3E801A05, publisher_id: 0x000(3), participant_id: 0x000(1) +[1724443531.237811] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x3E801A05, datawriter_id: 0x000(5), publisher_id: 0x000(3) +``` + +After connecting the ESP32-S2/S3 and the micro-ROS agent, you can list the topics: + +```bash +export ROS_DOMAIN_ID=100 # Set the ROS2 domain ID +ros2 topic list +``` + +Output expected: + +```bash +/esp32s2/int32_publisher_usbcdc +/parameter_events +/rosout +``` + +And see if the `esp32s2/int32_publisher_usbcdc` topic is available. You can echo the topic to see the messages: + +```bash +export ROS_DOMAIN_ID=100 # Set the ROS2 domain ID +ros2 topic echo esp32s2/int32_publisher_usbcdc +``` + +Output expected: + +```bash +data: 1 +--- +data: 2 +--- +data: 3 +--- +data: 4 +--- +data: 5 +. +. +. +``` + +To see the log output, you can use the following command: + +```bash +minicom -D /dev/ttyACM1 -b 115200 +``` + +Output expected: + +```bash +Welcome to minicom 2.8 + +OPTIONS: I18n +Port /dev/ttyACM1 +Press CTRL-A Z for help on special keys + +I (2688) MAIN: micro-ROS task created +I (2688) main_task: Returned from app_main() +I (3708) TIMER_CALLBACK: Message published: 0 +I (4708) TIMER_CALLBACK: Message published: 1 +I (5708) TIMER_CALLBACK: Message published: 2 +I (6708) TIMER_CALLBACK: Message published: 3 +I (7708) TIMER_CALLBACK: Message published: 4 +I (8708) TIMER_CALLBACK: Message published: 5 +. +. +. +``` diff --git a/examples/int32_publisher_custom_transport_usbcdc/app-colcon.meta b/examples/int32_publisher_custom_transport_usbcdc/app-colcon.meta new file mode 100644 index 00000000..906210cc --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/app-colcon.meta @@ -0,0 +1,9 @@ +{ + "names": { + "rmw_microxrcedds": { + "cmake-args": [ + "-DRMW_UXRCE_TRANSPORT=custom" + ] + } + } +} diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/CMakeLists.txt b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/CMakeLists.txt new file mode 100644 index 00000000..5d7b73a2 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/CMakeLists.txt @@ -0,0 +1 @@ +idf_component_register(SRCS "esp32s2_usbcdc_logging.c" INCLUDE_DIRS ".") diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.c b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.c new file mode 100644 index 00000000..f0c0c7de --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.c @@ -0,0 +1,46 @@ +#include "esp32s2_usbcdc_logging.h" + +// Initialize USB-CDC logging +esp_err_t esp32s2_usbcdc_logging_init(void) +{ + const tinyusb_config_t tinyusb_config = { + .descriptor = NULL, + .string_descriptor = NULL, + .external_phy = false, + .configuration_descriptor = NULL, + }; + + esp_err_t ret = tinyusb_driver_install(&tinyusb_config); + + if (ret == ESP_ERR_INVALID_ARG || ret == ESP_FAIL) { + return ret; + } + + tinyusb_config_cdcacm_t acm_config = { + .usb_dev = TINYUSB_USBDEV_0, + .cdc_port = TINYUSB_CDC_ACM_1, + .rx_unread_buf_sz = CONFIG_TINYUSB_CDC_RX_BUFSIZE, + .callback_rx = NULL, + .callback_rx_wanted_char = NULL, + .callback_line_state_changed = NULL, + .callback_line_coding_changed = NULL, + }; + + ret = tusb_cdc_acm_init(&acm_config); + + if (ret != ESP_OK) { + return ret; + } + + ret = esp_tusb_init_console(TINYUSB_CDC_ACM_1); + + return ret; +} + +// Deinitialize USB-CDC logging +esp_err_t esp32s2_usbcdc_logging_deinit(void) +{ + esp_err_t ret = esp_tusb_deinit_console(TINYUSB_CDC_ACM_1); + + return ret; +} diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.h b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.h new file mode 100644 index 00000000..ba8193da --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/esp32s2_usbcdc_logging.h @@ -0,0 +1,32 @@ +#ifndef ESP32S2_USBCDC_LOGGING_H +#define ESP32S2_USBCDC_LOGGING_H + +#include "esp_err.h" +#include "tinyusb.h" +#include "tusb_cdc_acm.h" +#include "tusb_console.h" +#include "sdkconfig.h" + +#if (CONFIG_TINYUSB_CDC_COUNT < 2) + #warning "Define CONFIG_TINYUSB_CDC_COUNT to 2 in menuconfig if you want log over USBCDC. Otherwise, disable log output in menuconfig." +#endif + +#if defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) + +#ifdef __cplusplus +extern "C" +{ +#endif + +esp_err_t esp32s2_usbcdc_logging_init(void); +esp_err_t esp32s2_usbcdc_logging_deinit(void); + +#ifdef __cplusplus +} +#endif + +#else +#error "Logging over USB-CDC is only supported on ESP32-S2 or ESP32-S3 targets" +#endif + +#endif // ESP32S2_USBCDC_LOGGING_H diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/idf_component.yml b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/idf_component.yml new file mode 100644 index 00000000..9e845ef0 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_logging/idf_component.yml @@ -0,0 +1,6 @@ +## IDF Component Manager Manifest File +dependencies: + espressif/esp_tinyusb: "^1.4.4" + ## Required IDF version + idf: + version: ">=5.0" diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/CMakeLists.txt b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/CMakeLists.txt new file mode 100644 index 00000000..256799bb --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register(SRCS "esp32s2_usbcdc_transport.c" + INCLUDE_DIRS "." + REQUIRES micro_ros_espidf_component # include +) diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.c b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.c new file mode 100644 index 00000000..7bde70d7 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.c @@ -0,0 +1,57 @@ +#include "esp32s2_usbcdc_transport.h" + +// Open USB-CDC +bool esp32s2_usbcdc_open(struct uxrCustomTransport* transport) { + const tinyusb_config_t tinyusb_config = { + .device_descriptor = NULL, + .string_descriptor = NULL, + .external_phy = false, + .configuration_descriptor = NULL, + }; + + esp_err_t ret = tinyusb_driver_install(&tinyusb_config); + + if (ret == ESP_ERR_INVALID_ARG || ret == ESP_FAIL) { + return ret; + } + + tinyusb_cdcacm_itf_t* cdc_port = (tinyusb_cdcacm_itf_t*)transport->args; + + tinyusb_config_cdcacm_t acm_cfg = { + .usb_dev = TINYUSB_USBDEV_0, + .cdc_port = *cdc_port, + .rx_unread_buf_sz = CONFIG_TINYUSB_CDC_RX_BUFSIZE, + .callback_rx = NULL, + .callback_rx_wanted_char = NULL, + .callback_line_state_changed = NULL, + .callback_line_coding_changed = NULL + }; + + if (tusb_cdc_acm_init(&acm_cfg) != ESP_OK) { + return false; + } + + return true; +} + +// Close USB-CDC +bool esp32s2_usbcdc_close(struct uxrCustomTransport* transport) { + tinyusb_cdcacm_itf_t* cdc_port = (tinyusb_cdcacm_itf_t*)transport->args; + return (tusb_cdc_acm_deinit(*cdc_port) == ESP_OK) ? true : false; +} + +// Write to USB-CDC +size_t esp32s2_usbcdc_write(struct uxrCustomTransport* transport, const uint8_t* buf, size_t len, uint8_t* err) { + tinyusb_cdcacm_itf_t* cdc_port = (tinyusb_cdcacm_itf_t*)transport->args; + size_t tx_size = tinyusb_cdcacm_write_queue(*cdc_port, buf, len); + tinyusb_cdcacm_write_flush(*cdc_port, 0); + return tx_size; +} + +// Read from USB-CDC +size_t esp32s2_usbcdc_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err) { + tinyusb_cdcacm_itf_t* cdc_port = (tinyusb_cdcacm_itf_t*)transport->args; + size_t rx_size = 0; + esp_err_t ret = tinyusb_cdcacm_read(*cdc_port, buf, len, &rx_size); + return (ret == ESP_OK) ? rx_size : 0; +} \ No newline at end of file diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.h b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.h new file mode 100644 index 00000000..eb20a202 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/esp32s2_usbcdc_transport.h @@ -0,0 +1,29 @@ +#ifndef ESP32S2_USBCDC_TRANSPORT_H +#define ESP32S2_USBCDC_TRANSPORT_H + +#include + +#include "tinyusb.h" +#include "tusb_cdc_acm.h" + +#if defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) + +#ifdef __cplusplus +extern "C" +{ +#endif + +bool esp32s2_usbcdc_open(struct uxrCustomTransport* transport); +bool esp32s2_usbcdc_close(struct uxrCustomTransport* transport); +size_t esp32s2_usbcdc_write(struct uxrCustomTransport* transport, const uint8_t* buf, size_t len, uint8_t* err); +size_t esp32s2_usbcdc_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); + +#ifdef __cplusplus +} +#endif + +#else +#error "This transport is only supported on ESP32-S2 or ESP32-S3 targets" +#endif // CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 + +#endif // ESP32S2_USBCDC_TRANSPORT_H \ No newline at end of file diff --git a/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/idf_component.yml b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/idf_component.yml new file mode 100644 index 00000000..9e845ef0 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/components/esp32s2_usbcdc_transport/idf_component.yml @@ -0,0 +1,6 @@ +## IDF Component Manager Manifest File +dependencies: + espressif/esp_tinyusb: "^1.4.4" + ## Required IDF version + idf: + version: ">=5.0" diff --git a/examples/int32_publisher_custom_transport_usbcdc/dependencies.lock b/examples/int32_publisher_custom_transport_usbcdc/dependencies.lock new file mode 100644 index 00000000..d0d9ab49 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/dependencies.lock @@ -0,0 +1,21 @@ +dependencies: + espressif/esp_tinyusb: + component_hash: f151d680d6847bfcfd5d8eb6d1c3ff926c208e6b963b2e83643a141bc70baa15 + source: + service_url: https://api.components.espressif.com/ + type: service + version: 1.4.4 + espressif/tinyusb: + component_hash: 214989d502fc168241a4a4f83b097d8ac44a93cd6f1787b4ac10069a8b3bebd3 + source: + service_url: https://api.components.espressif.com/ + type: service + version: 0.15.0~10 + idf: + component_hash: null + source: + type: idf + version: 5.1.2 +manifest_hash: 7f60fd8da9b1e2b73aafbc324107a75ce3928aa50666473c15511b764484240c +target: esp32s2 +version: 1.0.0 diff --git a/examples/int32_publisher_custom_transport_usbcdc/main/CMakeLists.txt b/examples/int32_publisher_custom_transport_usbcdc/main/CMakeLists.txt new file mode 100644 index 00000000..1c3fa240 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/main/CMakeLists.txt @@ -0,0 +1 @@ +idf_component_register(SRCS main.c INCLUDE_DIRS ".") \ No newline at end of file diff --git a/examples/int32_publisher_custom_transport_usbcdc/main/Kconfig.projbuild b/examples/int32_publisher_custom_transport_usbcdc/main/Kconfig.projbuild new file mode 100644 index 00000000..1ee6dcf2 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/main/Kconfig.projbuild @@ -0,0 +1,15 @@ +menu "micro-ROS example-app settings" + + config MICRO_ROS_APP_STACK + int "Stack the micro-ROS app (Bytes)" + default 16000 + help + Stack size in Bytes of the micro-ROS app + + config MICRO_ROS_APP_TASK_PRIO + int "Priority of the micro-ROS app" + default 5 + help + Priority of micro-ros task higher value means higher priority + +endmenu \ No newline at end of file diff --git a/examples/int32_publisher_custom_transport_usbcdc/main/component.mk b/examples/int32_publisher_custom_transport_usbcdc/main/component.mk new file mode 100644 index 00000000..0b9d7585 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/main/component.mk @@ -0,0 +1,5 @@ +# +# "main" pseudo-component makefile. +# +# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.) + diff --git a/examples/int32_publisher_custom_transport_usbcdc/main/main.c b/examples/int32_publisher_custom_transport_usbcdc/main/main.c new file mode 100644 index 00000000..f49bcc44 --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/main/main.c @@ -0,0 +1,166 @@ +#include +#include + +#include "driver/uart.h" +#include "esp_log.h" +#include "esp_system.h" +#include "esp32s2_usbcdc_logging.h" +#include "esp32s2_usbcdc_transport.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include +#include +#include +#include +#include +#include +#include + +#define DOMAIN_ID 100 +#define TIMER_PERIOD 1000 + +#define NODE_NAME "esp32s2" +#define PUBLISHER_NAME "/int32_publisher_usbcdc" +#define TOPIC_NAME NODE_NAME PUBLISHER_NAME + +#define NUMBER_OF_HANDLES 1 + +#define TAG_CALLBACK "TIMER_CALLBACK" +#define TAG_MAIN "MAIN" +#define TAG_TASK "MICRO_ROS" + +// Check for error in micro-ROS initialization, aborting the process +#define RCCHECK(fn) { \ + rcl_ret_t temp_rc = fn; \ + if ((temp_rc != RCL_RET_OK)) { \ + ESP_LOGE("RCCHECK", "Failed status on line %d: %d. Aborting.\n", __LINE__, (int)temp_rc); \ + vTaskDelete(NULL); \ + } \ +} + +// Check for error in micro-ROS initialization, continuing the process +#define RCSOFTCHECK(fn) { \ + rcl_ret_t temp_rc = fn; \ + if ((temp_rc != RCL_RET_OK)) { \ + ESP_LOGW("RCSOFTCHECK","Failed status on line %d: %d. Continuing.\n", __LINE__, (int)temp_rc); \ + } \ +} + +rcl_publisher_t publisher; // Publisher +std_msgs__msg__Int32 msg; // Message to be published + +// Timer callback. Publishes a message +void timer_callback(rcl_timer_t *timer, int64_t last_call_time) { + RCLC_UNUSED(last_call_time); + if (timer != NULL) { + // Publish message to topic + RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); + ESP_LOGI(TAG_CALLBACK, "Message published: %ld", msg.data++); + } +} + +void micro_ros_task(void *arg) { + + rcl_allocator_t allocator = rcl_get_default_allocator(); // Initialize allocator + rclc_support_t support = {0}; // Initialize support + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); // Initialize init_options + + // Initialize init_options + RCCHECK(rcl_init_options_init(&init_options, allocator)); + + // Set domain id + RCCHECK(rcl_init_options_set_domain_id(&init_options, DOMAIN_ID)); + + // Initialize support with options + RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator)); + + // Create node + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + RCCHECK(rclc_node_init_with_options(&node, NODE_NAME, "", &support, &node_ops)); + + // Create publisher + RCCHECK(rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + TOPIC_NAME)); + + // Create timer + rcl_timer_t timer; + RCCHECK(rclc_timer_init_default2( + &timer, + &support, + RCL_MS_TO_NS(TIMER_PERIOD), + timer_callback, + true)); + + // Create executor + rclc_executor_t executor; + RCCHECK(rclc_executor_init(&executor, &support.context, NUMBER_OF_HANDLES, &allocator)); + + // Add timer to executor + RCCHECK(rclc_executor_add_timer(&executor, &timer)); + + msg.data = 0; // Initialize message data + rcl_ret_t spin_ret; // Return code for spin + + while (true) { + spin_ret = rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); + if (spin_ret != RCL_RET_OK) { + ESP_LOGE(TAG_TASK, "rclc_executor_spin_some() failed"); + } + usleep(10000); + } + + // Free resources + RCCHECK(rcl_publisher_fini(&publisher, &node)); + RCCHECK(rcl_node_fini(&node)); + ESP_LOGE(TAG_TASK, "Task finished"); + + vTaskDelete(NULL); +} + +static tinyusb_cdcacm_itf_t cdc_port = TINYUSB_CDC_ACM_0; // interface USB-CDC + +void app_main(void) { + +// Initialize logging over USB-CDC +#if (CONFIG_TINYUSB_CDC_COUNT >= 2) + if (esp32s2_usbcdc_logging_init() == ESP_OK) { + ESP_LOGI(TAG_MAIN, "USB-CDC Logging initialized"); + } +#endif + +// Set micro-ROS custom transport layer +#if defined(RMW_UXRCE_TRANSPORT_CUSTOM) + rmw_ret_t ret = rmw_uros_set_custom_transport( + true, + (void *)&cdc_port, + esp32s2_usbcdc_open, + esp32s2_usbcdc_close, + esp32s2_usbcdc_write, + esp32s2_usbcdc_read); + + if (ret != RMW_RET_OK) { + ESP_LOGE(TAG_MAIN, "Fail to set micro-ROS custom transport layer"); + return; + } +#else +#error micro-ROS transports misconfigured +#endif // RMW_UXRCE_TRANSPORT_CUSTOM + + // Start micro-ROS task + TaskHandle_t task_handle = NULL; + xTaskCreate(micro_ros_task, + "uros_task", + CONFIG_MICRO_ROS_APP_STACK, + NULL, + CONFIG_MICRO_ROS_APP_TASK_PRIO, + &task_handle); + + if (task_handle != NULL) { + ESP_LOGI(TAG_MAIN, "micro-ROS task created"); + } +} \ No newline at end of file diff --git a/examples/int32_publisher_custom_transport_usbcdc/sdkconfig.defaults b/examples/int32_publisher_custom_transport_usbcdc/sdkconfig.defaults new file mode 100644 index 00000000..544d200d --- /dev/null +++ b/examples/int32_publisher_custom_transport_usbcdc/sdkconfig.defaults @@ -0,0 +1,6 @@ +CONFIG_ESP_MAIN_TASK_STACK_SIZE=3000 +CONFIG_ESP_TASK_WDT=n +CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE=y +CONFIG_MICRO_ROS_ESP_UART_TRANSPORT=y +CONFIG_TINYUSB_CDC_ENABLED=y +CONFIG_TINYUSB_CDC_COUNT=2 \ No newline at end of file