diff --git a/CMakeLists.txt b/CMakeLists.txt index 8560147..9cfd7bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,13 +2,9 @@ cmake_minimum_required(VERSION 3.20.0) SET(CMAKE_CXX_COMPILER /opt/zephyr-sdk/arm-zephyr-eabi/bin/arm-zephyr-eabi-gcc) -list(APPEND ZEPHYR_EXTRA_MODULES ${CMAKE_CURRENT_SOURCE_DIR}/modules/micro_ros_zephyr_module/modules/libmicroros) +list(APPEND ZEPHYR_EXTRA_MODULES ${CMAKE_CURRENT_SOURCE_DIR}/modules/libmicroros) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(sherpa_ros2_sensor_node) target_sources(app PRIVATE src/main.cpp) - -if(${BOARD} STREQUAL "native_sim") - add_subdirectory(src/emulation) -endif() diff --git a/boards/native_sim.conf b/boards/native_sim.conf deleted file mode 100644 index 3672159..0000000 --- a/boards/native_sim.conf +++ /dev/null @@ -1,12 +0,0 @@ -# gpio emulation -CONFIG_GPIO_EMUL=y -CONFIG_EMUL=y - -# network settings -CONFIG_ETH_NATIVE_POSIX=y -CONFIG_ETH_NATIVE_POSIX_DRV_NAME="eth0" - -CONFIG_NET_CONFIG_MY_IPV4_ADDR="10.43.30.100" -CONFIG_MICROROS_AGENT_IP="10.43.30.100" -CONFIG_NET_CONFIG_MY_IPV4_NETMASK="255.255.255.0" -CONFIG_NET_CONFIG_MY_IPV4_GW="10.43.30.1" \ No newline at end of file diff --git a/boards/native_sim.overlay b/boards/native_sim.overlay deleted file mode 100644 index 4c1662b..0000000 --- a/boards/native_sim.overlay +++ /dev/null @@ -1,97 +0,0 @@ -/ { - - aliases { - led0 = &user_led; - }; - - chosen { - zephyr,display = &gpio_visualizer_display; - }; - - leds { - compatible = "gpio-leds"; - user_led: user_led_0 { - gpios = <&gpioa_emul 0 GPIO_ACTIVE_HIGH>; - label = "LED 0"; - }; - }; - - gpioa_emul: gpioa { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOA"; - }; - - gpiob_emul: gpiob { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOB"; - }; - - gpioc_emul: gpioc { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOC"; - }; - - gpiod_emul: gpiod { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOD"; - }; - - gpioe_emul: gpioe { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOE"; - }; - - gpiof_emul: gpiof { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOF"; - }; - - gpiog_emul: gpiog { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOG"; - }; - - gpioh_emul: gpioh { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOH"; - }; - - gpioi_emul: gpioi { - compatible = "zephyr,gpio-emul"; - gpio-controller; - #gpio-cells = <2>; - ngpios = <16>; - label = "GPIOI"; - }; - - gpio_visualizer_display: sdl_dc { - compatible = "zephyr,sdl-dc"; - height = <240>; - width = <320>; - label = "GPIO DISPLAY"; - }; -}; \ No newline at end of file diff --git a/boards/nucleo_h563zi.conf b/boards/nucleo_h563zi.conf index 95b5c60..e69de29 100644 --- a/boards/nucleo_h563zi.conf +++ b/boards/nucleo_h563zi.conf @@ -1,8 +0,0 @@ -# network settings -CONFIG_ETH_STM32_HAL=y -CONFIG_NET_L2_ETHERNET=y - -CONFIG_NET_CONFIG_MY_IPV4_ADDR="10.42.30.100" -CONFIG_MICROROS_AGENT_IP="10.42.30.100" -CONFIG_NET_CONFIG_MY_IPV4_NETMASK="255.255.255.0" -CONFIG_NET_CONFIG_MY_IPV4_GW="10.42.30.1" \ No newline at end of file diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index ed31ec5..267997d 100644 --- a/boards/nucleo_h563zi.overlay +++ b/boards/nucleo_h563zi.overlay @@ -10,4 +10,15 @@ label = "USER_LED_0"; }; }; +}; + +&uart4 { + status = "okay"; + current-speed = <115200>; + pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>; + pinctrl-names = "default"; +}; + +&usart3 { + status = "okay"; }; \ No newline at end of file diff --git a/config.yml b/config.yml index cfc4c78..c6db9fb 100644 --- a/config.yml +++ b/config.yml @@ -6,7 +6,6 @@ common: min_flash: 16 tags: microros integration_platforms: - - native_sim - nucleo_h563zi harness: console harness_config: diff --git a/modules/libmicroros/.gitignore b/modules/libmicroros/.gitignore new file mode 100644 index 0000000..d450dd0 --- /dev/null +++ b/modules/libmicroros/.gitignore @@ -0,0 +1,6 @@ +micro_ros_dev +micro_ros_src +zephyr_toolchain.cmake +include +configured_colcon.meta +*.a \ No newline at end of file diff --git a/modules/libmicroros/CMakeLists.txt b/modules/libmicroros/CMakeLists.txt new file mode 100644 index 0000000..4e8c213 --- /dev/null +++ b/modules/libmicroros/CMakeLists.txt @@ -0,0 +1,107 @@ +# SPDX-License-Identifier: Apache-2.0 + +if(CONFIG_MICROROS) + +set(MICROROS_DIR ${ZEPHYR_CURRENT_MODULE_DIR}) + +if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") +set(submake "$(MAKE)") +else() +set(submake "make") +endif() + +# micro-ROS library + +zephyr_get_include_directories_for_lang_as_string( C includes) +zephyr_get_system_include_directories_for_lang_as_string(C system_includes) +zephyr_get_compile_definitions_for_lang_as_string( C definitions) +zephyr_get_compile_options_for_lang_as_string( C options) + +zephyr_get_include_directories_for_lang_as_string( CXX includes_cxx) +zephyr_get_system_include_directories_for_lang_as_string(CXX system_includes_cxx) +zephyr_get_compile_definitions_for_lang_as_string( CXX definitions_cxx) +zephyr_get_compile_options_for_lang_as_string( CXX options_cxx) + +set(external_project_cflags + "${includes} ${definitions} ${options} ${system_includes}" + ) + +set(external_project_cxxflags + "${includes_cxx} ${definitions_cxx} ${options_cxx} ${system_includes_cxx}" + ) + +include(ExternalProject) + +externalproject_add(libmicroros_project + PREFIX ${CMAKE_BINARY_DIR}/libmicroros-prefix + SOURCE_DIR ${MICROROS_DIR} + BINARY_DIR ${MICROROS_DIR} + CONFIGURE_COMMAND "" + BUILD_COMMAND + ${submake} -f libmicroros.mk + X_CFLAGS=${external_project_cflags} + X_CXXFLAGS=${external_project_cxxflags} + X_CC=${CMAKE_C_COMPILER} + X_AR=${CMAKE_AR} + X_RANLIB=${CMAKE_RANLIB} + X_CXX=${CMAKE_CXX_COMPILER} + COMPONENT_PATH=${MICROROS_DIR} + ZEPHYR_BASE=${ZEPHYR_BASE} + PROJECT_BINARY_DIR=${PROJECT_BINARY_DIR} + INSTALL_COMMAND "" + BUILD_BYPRODUCTS ${MICROROS_DIR}/libmicroros.a + ) + +zephyr_link_libraries(${MICROROS_DIR}/libmicroros.a) + +zephyr_interface_library_named(microros) +add_dependencies(microros libmicroros_project) +target_include_directories(microros INTERFACE ${MICROROS_DIR}/include) + +execute_process( + COMMAND + make -f libmicroros.mk get_package_names + COMPONENT_PATH=${MICROROS_DIR} + WORKING_DIRECTORY + ${MICROROS_DIR} + OUTPUT_VARIABLE + INCLUDE_ROS2_PACKAGES + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + +foreach(pkg ${INCLUDE_ROS2_PACKAGES}) + target_include_directories(microros INTERFACE ${MICROROS_DIR}/include/${pkg}) +endforeach() + +# micro-ROS transport library + +if(CONFIG_MICROROS_TRANSPORT_SERIAL) + set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/serial) +elseif(CONFIG_MICROROS_TRANSPORT_SERIAL_USB) + set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/serial-usb) +elseif(CONFIG_MICROROS_TRANSPORT_UDP) + set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/udp) +else() + message(FATAL_ERROR Please set a micro-ROS transport) +endif() + +zephyr_library_named(microros_transports) +zephyr_include_directories(${MICROROS_DIR}/include) +zephyr_include_directories(${MICROROS_TRANSPORT_DIR}) + +zephyr_library_sources( + ${MICROROS_TRANSPORT_DIR}/microros_transports.c +) + +add_dependencies(microros microros_transports) +add_dependencies(microros_transports libmicroros_project) + +# Cleaning + +set_property(DIRECTORY APPEND PROPERTY ADDITIONAL_MAKE_CLEAN_FILES + ${MICROROS_DIR}/include + ${MICROROS_DIR}/configured_colcon.meta + ${MICROROS_DIR}/zephyr_toolchain.cmake + ) + +endif() diff --git a/modules/libmicroros/Kconfig b/modules/libmicroros/Kconfig new file mode 100644 index 0000000..c421a8f --- /dev/null +++ b/modules/libmicroros/Kconfig @@ -0,0 +1,113 @@ +menuconfig MICROROS + bool + default n + prompt "micro-ROS Support" + select APP_LINK_WITH_MICROROS + help + Enables a micro-ROS library + +if MICROROS + config APP_LINK_WITH_MICROROS + bool "Link 'app' with MICROROS" + default n + help + Add MICROROS header files to the 'app' include path. + + choice + prompt "micro-ROS transport" + + config MICROROS_TRANSPORT_SERIAL + bool "micro-ROS serial transport" + select RING_BUFFER + config MICROROS_TRANSPORT_SERIAL_USB + bool "micro-ROS USB serial transport" + select RING_BUFFER + select USB_DEVICE_STACK + config MICROROS_TRANSPORT_UDP + bool "micro-ROS UDP network transport" + + endchoice + + if MICROROS_TRANSPORT_UDP + config MICROROS_AGENT_IP + string "micro-ROS Agent IP" + default "192.168.1.100" + help + micro-ROS Agent IP. + + config MICROROS_AGENT_PORT + string "micro-ROS Agent Port" + default "8888" + help + micro-ROS Agent port. + + menu "WiFi Configuration" + + config MICROROS_WIFI_SSID + string "WiFi SSID" + default "myssid" + help + SSID (network name) for the example to connect to. + + config MICROROS_WIFI_PASSWORD + string "WiFi Password" + default "mypassword" + help + WiFi password (WPA or WPA2) for the example to use. + endmenu + endif + + if MICROROS_TRANSPORT_SERIAL + config MICROROS_SERIAL_PORT + string "micro-ROS Agent serial port" + default "1" + help + micro-ROS Agent IP. + endif + + if MICROROS_TRANSPORT_SERIAL_USB + config USB_CDC_ACM + bool + default y + config USB_CDC_ACM_RINGBUF_SIZE + int "USB-CDC-ACM Ringbuffer size" + default "2048" + config USB_DEVICE_PRODUCT + string "USB Device Product" + default "Zephyr micro-ROS" + + endif + config MICROROS_NODES + string "available micro-ROS nodes" + default "4" + + config MICROROS_PUBLISHERS + string "available micro-ROS publishers" + default "2" + + config MICROROS_SUBSCRIBERS + string "available micro-ROS subscribers" + default "2" + + config MICROROS_CLIENTS + string "available micro-ROS service clients" + default "3" + + config MICROROS_SERVERS + string "available micro-ROS service servers" + default "1" + + config MICROROS_RMW_HISTORIC + string "available micro-ROS RMW historic memory" + default "4" + + config MICROROS_XRCE_DDS_MTU + string "micro-ROS transport MTU" + default "512" + + config MICROROS_XRCE_DDS_HISTORIC + string "micro-ROS middleware memory slots" + default "4" + +endif + diff --git a/modules/libmicroros/colcon.meta b/modules/libmicroros/colcon.meta new file mode 100644 index 0000000..70e1156 --- /dev/null +++ b/modules/libmicroros/colcon.meta @@ -0,0 +1,48 @@ +{ + "names": { + "tracetools": { + "cmake-args": [ + "-DTRACETOOLS_DISABLED=ON", + "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF" + ] + }, + "rcl":{ + "cmake-args":[ + "-DRCL_MICROROS=ON" + ] + }, + "rcutils": { + "cmake-args": [ + "-DENABLE_TESTING=OFF", + "-DRCUTILS_NO_FILESYSTEM=ON", + "-DRCUTILS_NO_THREAD_SUPPORT=ON", + "-DRCUTILS_AVOID_DYNAMIC_ALLOCATION=ON", + "-DRCUTILS_NO_64_ATOMIC=ON" + ] + }, + "microxrcedds_client": { + "cmake-args": [ + "-DUCLIENT_PIC=OFF", + "-DUCLIENT_PROFILE_DISCOVERY=OFF", + "-DUCLIENT_EXTERNAL_SERIAL=ON" + ] + }, + "rmw_microxrcedds": { + "cmake-args": [ + "-DRMW_UXRCE_MAX_NODES=1", + "-DRMW_UXRCE_MAX_PUBLISHERS=1", + "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0", + "-DRMW_UXRCE_MAX_HISTORY=4", + "-DRMW_UXRCE_MAX_SERVICES=0", + "-DRMW_UXRCE_MAX_CLIENTS=0", + "-DRMW_UXRCE_TRANSPORT=custom_serial" + ] + }, + "tracetools": { + "cmake-args": [ + "-DTRACETOOLS_DISABLED=ON", + "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF" + ] + } + } +} diff --git a/modules/libmicroros/libmicroros.mk b/modules/libmicroros/libmicroros.mk new file mode 100644 index 0000000..09467d3 --- /dev/null +++ b/modules/libmicroros/libmicroros.mk @@ -0,0 +1,136 @@ +UROS_DIR = $(COMPONENT_PATH)/micro_ros_src +DEBUG ?= 0 + +ifeq ($(DEBUG), 1) + BUILD_TYPE = Debug +else + BUILD_TYPE = Release +endif + +CFLAGS_INTERNAL := $(X_CFLAGS) +CXXFLAGS_INTERNAL := $(X_CXXFLAGS) + +CFLAGS_INTERNAL := -c -I$(ZEPHYR_BASE)/include/posix -I$(PROJECT_BINARY_DIR)/include/generated $(CFLAGS_INTERNAL) +CXXFLAGS_INTERNAL := -c -I$(ZEPHYR_BASE)/include/posix -I$(PROJECT_BINARY_DIR)/include/generated $(CXXFLAGS_INTERNAL) + +all: $(COMPONENT_PATH)/libmicroros.a + +clean: + rm -rf $(COMPONENT_PATH)/libmicroros.a; \ + rm -rf $(COMPONENT_PATH)/include; \ + rm -rf $(COMPONENT_PATH)/zephyr_toolchain.cmake; \ + rm -rf $(COMPONENT_PATH)/micro_ros_dev; \ + rm -rf $(COMPONENT_PATH)/micro_ros_src; + +ZEPHYR_CONF_FILE := $(PROJECT_BINARY_DIR)/.config + +get_package_names: $(COMPONENT_PATH)/micro_ros_src/src + @cd $(COMPONENT_PATH)/micro_ros_src/src; \ + colcon list | awk '{print $$1}' | awk -v d=";" '{s=(NR==1?s:s d)$$0}END{print s}' + +configure_colcon_meta: $(COMPONENT_PATH)/colcon.meta $(COMPONENT_PATH)/micro_ros_src/src + . $(COMPONENT_PATH)/utils.sh; \ + cp $(COMPONENT_PATH)/colcon.meta $(COMPONENT_PATH)/configured_colcon.meta; \ + ZEPHYR_CONF_FILE=$(ZEPHYR_CONF_FILE); \ + update_meta_from_zephyr_config "CONFIG_MICROROS_NODES" "rmw_microxrcedds" "RMW_UXRCE_MAX_NODES"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_PUBLISHERS" "rmw_microxrcedds" "RMW_UXRCE_MAX_PUBLISHERS"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_SUBSCRIBERS" "rmw_microxrcedds" "RMW_UXRCE_MAX_SUBSCRIPTIONS"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_CLIENTS" "rmw_microxrcedds" "RMW_UXRCE_MAX_CLIENTS"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_SERVERS" "rmw_microxrcedds" "RMW_UXRCE_MAX_SERVICES"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_RMW_HISTORIC" "rmw_microxrcedds" "RMW_UXRCE_MAX_HISTORY"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_XRCE_DDS_HISTORIC" "rmw_microxrcedds" "RMW_UXRCE_STREAM_HISTORY"; \ + update_meta_from_zephyr_config "CONFIG_MICROROS_XRCE_DDS_MTU" "microxrcedds_client" "UCLIENT_CUSTOM_TRANSPORT_MTU"; \ + update_meta "microxrcedds_client" "UCLIENT_PROFILE_SERIAL=OFF"; \ + update_meta "microxrcedds_client" "UCLIENT_PROFILE_UDP=OFF"; \ + update_meta "microxrcedds_client" "UCLIENT_PROFILE_TCP=OFF"; \ + update_meta "microxrcedds_client" "UCLIENT_PROFILE_CUSTOM_TRANSPORT=ON"; \ + update_meta "microxrcedds_client" "UCLIENT_PROFILE_STREAM_FRAMING=ON"; \ + update_meta "rmw_microxrcedds" "RMW_UXRCE_TRANSPORT=custom"; + + +configure_toolchain: $(COMPONENT_PATH)/zephyr_toolchain.cmake.in + rm -f $(COMPONENT_PATH)/zephyr_toolchain.cmake; \ + cat $(COMPONENT_PATH)/zephyr_toolchain.cmake.in | \ + sed "s/@CMAKE_C_COMPILER@/$(subst /,\/,$(X_CC))/g" | \ + sed "s/@CMAKE_CXX_COMPILER@/$(subst /,\/,$(X_CXX))/g" | \ + sed "s/@CMAKE_SYSROOT@/$(subst /,\/,$(COMPONENT_PATH))/g" | \ + sed "s/@CFLAGS@/$(subst /,\/,$(CFLAGS_INTERNAL))/g" | \ + sed "s/@CXXFLAGS@/$(subst /,\/,$(CXXFLAGS_INTERNAL))/g" \ + > $(COMPONENT_PATH)/zephyr_toolchain.cmake + +$(COMPONENT_PATH)/micro_ros_dev/install: + rm -rf micro_ros_dev; \ + mkdir micro_ros_dev; cd micro_ros_dev; \ + git clone -b jazzy https://github.com/ament/ament_cmake src/ament_cmake; \ + git clone -b jazzy https://github.com/ament/ament_lint src/ament_lint; \ + git clone -b jazzy https://github.com/ament/ament_package src/ament_package; \ + git clone -b jazzy https://github.com/ament/googletest src/googletest; \ + git clone -b jazzy https://github.com/ros2/ament_cmake_ros src/ament_cmake_ros; \ + git clone -b jazzy https://github.com/ament/ament_index src/ament_index; \ + colcon build --cmake-args -DBUILD_TESTING=OFF; + +$(COMPONENT_PATH)/micro_ros_src/src: + @rm -rf micro_ros_src; \ + mkdir micro_ros_src; cd micro_ros_src; \ + git clone -b ros2 https://github.com/eProsima/micro-CDR src/micro-CDR; \ + git clone -b ros2 https://github.com/eProsima/Micro-XRCE-DDS-Client src/Micro-XRCE-DDS-Client; \ + git clone -b jazzy https://github.com/micro-ROS/rcl src/rcl; \ + git clone -b jazzy https://github.com/ros2/rclc src/rclc; \ + git clone -b jazzy https://github.com/micro-ROS/rcutils src/rcutils; \ + git clone -b jazzy https://github.com/micro-ROS/micro_ros_msgs src/micro_ros_msgs; \ + git clone -b jazzy https://github.com/micro-ROS/rmw-microxrcedds src/rmw-microxrcedds; \ + git clone -b jazzy https://github.com/micro-ROS/rosidl_typesupport src/rosidl_typesupport; \ + git clone -b jazzy https://github.com/micro-ROS/rosidl_typesupport_microxrcedds src/rosidl_typesupport_microxrcedds; \ + git clone -b jazzy https://github.com/ros2/rosidl src/rosidl; \ + git clone -b jazzy https://github.com/ros2/rosidl_dynamic_typesupport src/rosidl_dynamic_typesupport; \ + git clone -b jazzy https://github.com/ros2/rmw src/rmw; \ + git clone -b jazzy https://github.com/ros2/rcl_interfaces src/rcl_interfaces; \ + git clone -b jazzy https://github.com/ros2/rosidl_defaults src/rosidl_defaults; \ + git clone -b jazzy https://github.com/ros2/unique_identifier_msgs src/unique_identifier_msgs; \ + git clone -b jazzy https://github.com/ros2/common_interfaces src/common_interfaces; \ + git clone -b jazzy https://github.com/ros2/test_interface_files src/test_interface_files; \ + git clone -b jazzy https://github.com/ros2/rmw_implementation src/rmw_implementation; \ + git clone -b jazzy https://github.com/ros2/rcl_logging src/rcl_logging; \ + git clone -b jazzy https://github.com/ros2/ros2_tracing src/ros2_tracing; \ + git clone -b jazzy https://github.com/micro-ROS/micro_ros_utilities src/micro_ros_utilities; \ + git clone -b jazzy https://github.com/ros2/rosidl_core src/rosidl_core; \ + touch src/ros2_tracing/test_tracetools/COLCON_IGNORE; \ + touch src/ros2_tracing/lttngpy/COLCON_IGNORE; \ + touch src/rosidl/rosidl_typesupport_introspection_cpp/COLCON_IGNORE; \ + touch src/rclc/rclc_examples/COLCON_IGNORE; \ + touch src/common_interfaces/actionlib_msgs/COLCON_IGNORE; \ + touch src/common_interfaces/std_srvs/COLCON_IGNORE; \ + touch src/rcl/rcl_yaml_param_parser/COLCON_IGNORE; \ + touch src/rcl_logging/rcl_logging_spdlog/COLCON_IGNORE; \ + touch src/rcl_interfaces/test_msgs/COLCON_IGNORE; + +$(COMPONENT_PATH)/micro_ros_src/install: configure_colcon_meta configure_toolchain $(COMPONENT_PATH)/micro_ros_dev/install $(COMPONENT_PATH)/micro_ros_src/src + cd $(UROS_DIR); \ + . ../micro_ros_dev/install/local_setup.sh; \ + colcon build \ + --merge-install \ + --packages-ignore-regex=.*_cpp \ + --metas $(COMPONENT_PATH)/configured_colcon.meta \ + --cmake-args \ + "--no-warn-unused-cli" \ + -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=OFF \ + -DTHIRDPARTY=ON \ + -DBUILD_SHARED_LIBS=OFF \ + -DBUILD_TESTING=OFF \ + -DCMAKE_BUILD_TYPE=$(BUILD_TYPE) \ + -DCMAKE_TOOLCHAIN_FILE=$(COMPONENT_PATH)/zephyr_toolchain.cmake \ + -DCMAKE_VERBOSE_MAKEFILE=OFF; \ + +$(COMPONENT_PATH)/libmicroros.a: $(COMPONENT_PATH)/micro_ros_src/install + mkdir -p $(UROS_DIR)/libmicroros; cd $(UROS_DIR)/libmicroros; \ + for file in $$(find $(UROS_DIR)/install/lib/ -name '*.a'); do \ + folder=$$(echo $$file | sed -E "s/(.+)\/(.+).a/\2/"); \ + mkdir -p $$folder; cd $$folder; $(X_AR) x $$file; \ + for f in *; do \ + mv $$f ../$$folder-$$f; \ + done; \ + cd ..; rm -rf $$folder; \ + done ; \ + $(X_AR) rc libmicroros.a *.obj; cp libmicroros.a $(COMPONENT_PATH); ${X_RANLIB} $(COMPONENT_PATH)/libmicroros.a; \ + cd ..; rm -rf libmicroros; \ + cp -R $(UROS_DIR)/install/include $(COMPONENT_PATH)/include; diff --git a/modules/libmicroros/microros_transports/serial-usb/microros_transports.c b/modules/libmicroros/microros_transports/serial-usb/microros_transports.c new file mode 100644 index 0000000..289e382 --- /dev/null +++ b/modules/libmicroros/microros_transports/serial-usb/microros_transports.c @@ -0,0 +1,168 @@ +#include + +#include +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#include +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +#include + + +#define RING_BUF_SIZE 2048 + +char uart_in_buffer[RING_BUF_SIZE]; +char uart_out_buffer[RING_BUF_SIZE]; + +struct ring_buf out_ringbuf, in_ringbuf; + +static void uart_fifo_callback(const struct device *dev, void * user_data){ + while (uart_irq_update(dev) && uart_irq_is_pending(dev)) { + if (uart_irq_rx_ready(dev)) { + int recv_len; + char buffer[64]; + size_t len = MIN(ring_buf_space_get(&in_ringbuf), sizeof(buffer)); + + if (len > 0){ + recv_len = uart_fifo_read(dev, buffer, len); + ring_buf_put(&in_ringbuf, buffer, recv_len); + } + + } + + if (uart_irq_tx_ready(dev)) { + char buffer[64]; + int rb_len; + + rb_len = ring_buf_get(&out_ringbuf, buffer, sizeof(buffer)); + + if (rb_len == 0) { + uart_irq_tx_disable(dev); + continue; + } + + uart_fifo_fill(dev, buffer, rb_len); + } + } +} + +bool zephyr_transport_open(struct uxrCustomTransport * transport){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + int ret; + uint32_t baudrate, dtr = 0U; + + + params->uart_dev = device_get_binding("CDC_ACM_0"); + if (!params->uart_dev) { + printk("CDC ACM device not found\n"); + return false; + } + + ret = usb_enable(NULL); + if (ret != 0) { + printk("Failed to enable USB\n"); + return false; + } + + ring_buf_init(&out_ringbuf, sizeof(uart_out_buffer), uart_out_buffer); + ring_buf_init(&in_ringbuf, sizeof(uart_in_buffer), uart_out_buffer); + + printk("Waiting for agent connection\n"); + + while (true) { + uart_line_ctrl_get(params->uart_dev, UART_LINE_CTRL_DTR, &dtr); + if (dtr) { + break; + } else { + /* Give CPU resources to low priority threads. */ + k_sleep(K_MSEC(100)); + } + } + + printk("Serial port connected!\n"); + + /* They are optional, we use them to test the interrupt endpoint */ + ret = uart_line_ctrl_set(params->uart_dev, UART_LINE_CTRL_DCD, 1); + if (ret) { + printk("Failed to set DCD, ret code %d\n", ret); + } + + ret = uart_line_ctrl_set(params->uart_dev, UART_LINE_CTRL_DSR, 1); + if (ret) { + printk("Failed to set DSR, ret code %d\n", ret); + } + + /* Wait 1 sec for the host to do all settings */ + k_busy_wait(1000*1000); + + ret = uart_line_ctrl_get(params->uart_dev, UART_LINE_CTRL_BAUD_RATE, &baudrate); + if (ret) { + printk("Failed to get baudrate, ret code %d\n", ret); + } + + uart_irq_callback_set(params->uart_dev, uart_fifo_callback); + + /* Enable rx interrupts */ + uart_irq_rx_enable(params->uart_dev); + + return true; +} + +bool zephyr_transport_close(struct uxrCustomTransport * transport){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + (void) params; + + return true; +} + +size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + size_t wrote; + + wrote = ring_buf_put(&out_ringbuf, buf, len); + + uart_irq_tx_enable(params->uart_dev); + + while (!ring_buf_is_empty(&out_ringbuf)){ + k_sleep(K_MSEC(5)); + } + + return wrote; +} + +size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + size_t read = 0; + int spent_time = 0; + + while(ring_buf_is_empty(&in_ringbuf) && spent_time < timeout){ + k_sleep(K_MSEC(1)); + spent_time++; + } + + uart_irq_rx_disable(params->uart_dev); + read = ring_buf_get(&in_ringbuf, buf, len); + uart_irq_rx_enable(params->uart_dev); + + return read; +} \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/serial-usb/microros_transports.h b/modules/libmicroros/microros_transports/serial-usb/microros_transports.h new file mode 100644 index 0000000..ff76070 --- /dev/null +++ b/modules/libmicroros/microros_transports/serial-usb/microros_transports.h @@ -0,0 +1,49 @@ +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ +#define _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ + +#include +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct { + size_t fd; + const struct device *uart_dev; +} zephyr_transport_params_t; + +#define MICRO_ROS_FRAMING_REQUIRED true +static zephyr_transport_params_t default_params; + +bool zephyr_transport_open(struct uxrCustomTransport * transport); +bool zephyr_transport_close(struct uxrCustomTransport * transport); +size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err); +size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); + +#ifdef __cplusplus +} +#endif + +#endif //_MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/serial/microros_transports.c b/modules/libmicroros/microros_transports/serial/microros_transports.c new file mode 100644 index 0000000..fe2ccf7 --- /dev/null +++ b/modules/libmicroros/microros_transports/serial/microros_transports.c @@ -0,0 +1,123 @@ +#include + +#include +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +#define RING_BUF_SIZE 2048 +#define UART_NODE DT_NODELABEL(uart4) + +char uart_in_buffer[RING_BUF_SIZE]; +char uart_out_buffer[RING_BUF_SIZE]; + +struct ring_buf out_ringbuf, in_ringbuf; + +// --- micro-ROS Serial Transport for Zephyr --- + +static void uart_fifo_callback(const struct device * dev, void * args){ + while (uart_irq_update(dev) && uart_irq_is_pending(dev)) { + if (uart_irq_rx_ready(dev)) { + int recv_len; + char buffer[64]; + size_t len = MIN(ring_buf_space_get(&in_ringbuf), sizeof(buffer)); + + if (len > 0){ + recv_len = uart_fifo_read(dev, buffer, len); + ring_buf_put(&in_ringbuf, buffer, recv_len); + } + } + } +} + + +bool zephyr_transport_open(struct uxrCustomTransport * transport){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + params->uart_dev = DEVICE_DT_GET(UART_NODE); + if (!params->uart_dev) { + printk("Serial device not found\n"); + return false; + } + + if (!device_is_ready(params->uart_dev)) { + printk("Serial device not ready\n"); + return false; + } + + printk("Serial device connected\n"); + + ring_buf_init(&in_ringbuf, sizeof(uart_in_buffer), uart_in_buffer); + + uart_irq_callback_user_data_set(params->uart_dev, uart_fifo_callback, NULL); + + /* Enable rx interrupts */ + uart_irq_rx_enable(params->uart_dev); + + return true; +} + +bool zephyr_transport_close(struct uxrCustomTransport * transport){ + (void) transport; + // TODO: close serial transport here + return true; +} + +size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + printf("TX: "); + for (size_t i = 0; i < len; i++) { + printf("%02X ", buf[i]); + } + printf("\n"); + + for (size_t i = 0; i < len; i++) + { + uart_poll_out(params->uart_dev, buf[i]); + } + + return len; +} + +size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + size_t read = 0; + int spent_time = 0; + + while(ring_buf_is_empty(&in_ringbuf) && spent_time < timeout){ + usleep(1000); + spent_time++; + } + + uart_irq_rx_disable(params->uart_dev); + read = ring_buf_get(&in_ringbuf, buf, len); + uart_irq_rx_enable(params->uart_dev); + + printf("RX: "); + for (size_t i = 0; i < read; i++) { + printf("%02X ", buf[i]); + } + printf("\n"); + + return read; +} \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/serial/microros_transports.h b/modules/libmicroros/microros_transports/serial/microros_transports.h new file mode 100644 index 0000000..8889d47 --- /dev/null +++ b/modules/libmicroros/microros_transports/serial/microros_transports.h @@ -0,0 +1,49 @@ +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ +#define _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ + +#include +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct { + size_t fd; + const struct device * uart_dev; +} zephyr_transport_params_t; + +#define MICRO_ROS_FRAMING_REQUIRED true +volatile static zephyr_transport_params_t default_params = {.fd = 1}; + +bool zephyr_transport_open(struct uxrCustomTransport * transport); +bool zephyr_transport_close(struct uxrCustomTransport * transport); +size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err); +size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); + +#ifdef __cplusplus +} +#endif + +#endif //_MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/udp/microros_transports.c b/modules/libmicroros/microros_transports/udp/microros_transports.c new file mode 100644 index 0000000..925a51e --- /dev/null +++ b/modules/libmicroros/microros_transports/udp/microros_transports.c @@ -0,0 +1,103 @@ +#include + +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#endif + +#include +#include +#include +#include + +#include + +bool zephyr_transport_open(struct uxrCustomTransport * transport){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + bool rv = false; + params->poll_fd.fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + + if (-1 != params->poll_fd.fd) + { + struct addrinfo hints; + struct addrinfo* result; + struct addrinfo* ptr; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_DGRAM; + + if (0 == getaddrinfo(params->ip, params->port, &hints, &result)) + { + for (ptr = result; ptr != NULL; ptr = ptr->ai_next) + { + if (0 == connect(params->poll_fd.fd, ptr->ai_addr, ptr->ai_addrlen)) + { + params->poll_fd.events = POLLIN; + rv = true; + break; + } + } + } + freeaddrinfo(result); + } + return rv; +} + +bool zephyr_transport_close(struct uxrCustomTransport * transport){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + return (-1 == params->poll_fd.fd) ? true : (0 == close(params->poll_fd.fd)); +} + +size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + size_t rv = 0; + ssize_t bytes_sent = send(params->poll_fd.fd, (void*)buf, len, 0); + if (-1 != bytes_sent) + { + rv = (size_t)bytes_sent; + *err = 0; + } + else + { + *err = 1; + } + return rv; +} + +size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){ + zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + + size_t rv = 0; + int poll_rv = poll(¶ms->poll_fd, 1, timeout); + if (0 < poll_rv) + { + ssize_t bytes_received = recv(params->poll_fd.fd, (void*)buf, len, 0); + if (-1 != bytes_received) + { + rv = (size_t)bytes_received; + *err = 0; + } + else + { + *err = 1; + } + } + else + { + *err = (0 == poll_rv) ? 0 : 1; + } + return rv; +} \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/udp/microros_transports.h b/modules/libmicroros/microros_transports/udp/microros_transports.h new file mode 100644 index 0000000..760a905 --- /dev/null +++ b/modules/libmicroros/microros_transports/udp/microros_transports.h @@ -0,0 +1,47 @@ +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ +#define _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ + +#include + +#include +#include +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct { + struct pollfd poll_fd; + char ip[16]; + char port[6]; +} zephyr_transport_params_t; + +#define MICRO_ROS_FRAMING_REQUIRED false +static zephyr_transport_params_t default_params = {{0,0,0}, "192.168.1.100", "8888"}; + +bool zephyr_transport_open(struct uxrCustomTransport * transport); +bool zephyr_transport_close(struct uxrCustomTransport * transport); +size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err); +size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); + +#ifdef __cplusplus +} +#endif + +#endif //_MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_ \ No newline at end of file diff --git a/modules/libmicroros/utils.sh b/modules/libmicroros/utils.sh new file mode 100644 index 0000000..09b2089 --- /dev/null +++ b/modules/libmicroros/utils.sh @@ -0,0 +1,14 @@ +update_meta() { + python3 -c "import sys; import json; c = '-D' +'$2'; s = json.loads(''.join([l for l in sys.stdin])); k = s['names']['$1']['cmake-args']; i = [c.startswith(v.split('=')[0]) for v in k]; k.pop(i.index(True)) if any(i) else None; k.append(c) if len(c.split('=')[1]) else None; print(json.dumps(s,indent=4))" < $COMPONENT_PATH/configured_colcon.meta > $COMPONENT_PATH/configured_colcon_new.meta + mv $COMPONENT_PATH/configured_colcon_new.meta $COMPONENT_PATH/configured_colcon.meta +} + +remove_meta() { + python3 -c "import sys; import json; c = '-D' +'$2'; s = json.loads(''.join([l for l in sys.stdin])); k = s['names']['$1']['cmake-args']; i = [c.startswith(v.split('=')[0]) for v in k]; k.pop(i.index(True)) if any(i) else None; print(json.dumps(s,indent=4))" < $COMPONENT_PATH/configured_colcon.meta > $COMPONENT_PATH/configured_colcon_new.meta + mv $COMPONENT_PATH/configured_colcon_new.meta $COMPONENT_PATH/configured_colcon.meta +} + +update_meta_from_zephyr_config() { + AUX_VARIABLE=$(grep $1 $ZEPHYR_CONF_FILE | awk -F '=' '{gsub(/"/, "", $2); print $2}'); \ + update_meta "$2" "$3=$AUX_VARIABLE"; \ +} diff --git a/modules/libmicroros/zephyr/module.yml b/modules/libmicroros/zephyr/module.yml new file mode 100644 index 0000000..c5fb2c4 --- /dev/null +++ b/modules/libmicroros/zephyr/module.yml @@ -0,0 +1,3 @@ +build: + cmake: . + kconfig: Kconfig \ No newline at end of file diff --git a/modules/libmicroros/zephyr_toolchain.cmake.in b/modules/libmicroros/zephyr_toolchain.cmake.in new file mode 100644 index 0000000..5ddbc44 --- /dev/null +++ b/modules/libmicroros/zephyr_toolchain.cmake.in @@ -0,0 +1,34 @@ +include(CMakeForceCompiler) + +set(CMAKE_SYSTEM_NAME Generic) +# set(CMAKE_SYSTEM_PROCESSOR arm) +set(CMAKE_CROSSCOMPILING 1) +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) +set(PLATFORM_NAME "zephyr") + +set(CMAKE_SYSROOT @CMAKE_SYSROOT@) + +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# Makefile flags + +set(CMAKE_THREAD_LIBS_INIT "-lpthread") +set(CMAKE_HAVE_THREADS_LIBRARY 1) +set(CMAKE_USE_WIN32_THREADS_INIT 0) +set(CMAKE_USE_PTHREADS_INIT 1) +set(THREADS_PREFER_PTHREAD_FLAG ON) + +SET(CMAKE_C_COMPILER_WORKS 1 CACHE INTERNAL "") +SET(CMAKE_CXX_COMPILER_WORKS 1 CACHE INTERNAL "") + +set(CMAKE_C_COMPILER @CMAKE_C_COMPILER@) +set(CMAKE_CXX_COMPILER @CMAKE_CXX_COMPILER@) +#set(CMAKE_C_COMPILER /opt/zephyr-sdk/arm-zephyr-eabi/bin/arm-zephyr-eabi-gcc) +#set(CMAKE_CXX_COMPILER /opt/zephyr-sdk/arm-zephyr-eabi/bin/arm-zephyr-eabi-g++) + +set(CMAKE_C_FLAGS_INIT "@CFLAGS@" CACHE STRING "" FORCE) +set(CMAKE_CXX_FLAGS_INIT "@CXXFLAGS@" CACHE STRING "" FORCE) + +set(__BIG_ENDIAN__ 0) \ No newline at end of file diff --git a/modules/micro_ros_zephyr_module b/modules/micro_ros_zephyr_module deleted file mode 160000 index d1e19be..0000000 --- a/modules/micro_ros_zephyr_module +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d1e19be708360a58250f287a2e1f5c61526cfb2d diff --git a/prj.conf b/prj.conf index a0e5cc8..da986dd 100644 --- a/prj.conf +++ b/prj.conf @@ -1,4 +1,5 @@ # c version +CONFIG_MAIN_STACK_SIZE=8192 CONFIG_CPP=y CONFIG_NEWLIB_LIBC=y CONFIG_NEWLIB_LIBC_NANO=n @@ -7,27 +8,15 @@ CONFIG_MINIMAL_LIBC=n CONFIG_POSIX_API=y # logging -CONFIG_LOG=n +CONFIG_LOG=y # gpio support CONFIG_GPIO=y -# display for native_sim -CONFIG_DISPLAY=y -CONFIG_SDL_DISPLAY=y +# usb +CONFIG_UART_INTERRUPT_DRIVEN=y -# networking -CONFIG_NETWORKING=y -CONFIG_NET_CONFIG_SETTINGS=y -CONFIG_NET_CONFIG_AUTO_INIT=y -CONFIG_NET_CONFIG_NEED_IPV4=y - -CONFIG_NET_IPV4=y -CONFIG_NET_IPV6=n -CONFIG_NET_DHCPV4=n -CONFIG_NET_UDP=y -CONFIG_NET_TCP=n # micro ros CONFIG_MICROROS=y -CONFIG_MICROROS_TRANSPORT_UDP=y \ No newline at end of file +CONFIG_MICROROS_TRANSPORT_SERIAL=y \ No newline at end of file diff --git a/src/emulation/CMakeLists.txt b/src/emulation/CMakeLists.txt deleted file mode 100644 index 8769251..0000000 --- a/src/emulation/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -target_sources(app PRIVATE gpio_visualizer.cpp) - diff --git a/src/emulation/gpio_visualizer.cpp b/src/emulation/gpio_visualizer.cpp deleted file mode 100644 index 4373a12..0000000 --- a/src/emulation/gpio_visualizer.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include "gpio_visualizer.hpp" - -#include -#include -#include -#include -#include -#include - -#define GPIO_VISUALIZER_STACK_SIZE 64 * 1024 * 1024 // 64 MB -#define GPIO_VISUALIZER_PRIORITY 10 - -#define NR_OF_GPIO_PORTS 9 -#define NR_OF_GPIO_PINS_PER_PORT 16 - -#define DISPLAY_WIDTH 320 -#define DISPLAY_HEIGHT 240 -#define DISPLAY_COLOR_DEPTH 4 -#define GPIO_PIXEL_SIZE 20 - -K_THREAD_STACK_DEFINE(gpio_visualizer__thread_stack, GPIO_VISUALIZER_STACK_SIZE); -struct k_thread gpio_visualizer_thread_data; - -const struct device *gpioa = device_get_binding("GPIOA"); -const struct device *gpiob = device_get_binding("GPIOB"); -const struct device *gpioc = device_get_binding("GPIOC"); -const struct device *gpiod = device_get_binding("GPIOD"); -const struct device *gpioe = device_get_binding("GPIOE"); -const struct device *gpiof = device_get_binding("GPIOF"); -const struct device *gpiog = device_get_binding("GPIOG"); -const struct device *gpioh = device_get_binding("GPIOH"); -const struct device *gpioi = device_get_binding("GPIOI"); - - -void draw_led(uint8_t port, uint8_t pin, uint8_t state, uint8_t *framebuf) { - uint32_t x_base = GPIO_PIXEL_SIZE * pin; - uint32_t y_base = GPIO_PIXEL_SIZE * port; - - for (uint32_t i = 0; i < GPIO_PIXEL_SIZE * GPIO_PIXEL_SIZE; i++) { - uint32_t x = x_base + (i % GPIO_PIXEL_SIZE); - uint32_t y = y_base + (i / GPIO_PIXEL_SIZE); - framebuf[x * DISPLAY_COLOR_DEPTH + 2 + DISPLAY_WIDTH * DISPLAY_COLOR_DEPTH * y] = state * 0xFF; - } -} - -void gpio_visualizer_thread(void *ARG1, void *ARG2, void *ARG3) { - - const struct device* gpio_ports[NR_OF_GPIO_PORTS] = {gpioa, gpiob, gpioc, gpiod, gpioe, gpiof, gpiog, gpioh, gpioi}; - const struct device *gpio_display = device_get_binding("GPIO DISPLAY"); - struct display_buffer_descriptor desc; - uint8_t buf[DISPLAY_WIDTH * DISPLAY_HEIGHT * DISPLAY_COLOR_DEPTH] = {0}; - - desc.buf_size = sizeof(buf); - desc.width = DISPLAY_WIDTH; - desc.height = DISPLAY_HEIGHT; - desc.pitch = DISPLAY_WIDTH; - - for (uint32_t i = 0; i < desc.buf_size; i++) { - if (i % DISPLAY_COLOR_DEPTH == DISPLAY_COLOR_DEPTH - 1) { - buf[i] = 0xFF; - } - } - - display_set_contrast(gpio_display, 100); - display_blanking_off(gpio_display); - - int running = 1; - while (running) { - - for (uint8_t port_idx = 0; port_idx < NR_OF_GPIO_PORTS; port_idx++) { - for (uint8_t pin_idx = 0; pin_idx < NR_OF_GPIO_PINS_PER_PORT; pin_idx++) { - uint8_t state = gpio_emul_output_get(gpio_ports[port_idx], pin_idx); - draw_led(port_idx, pin_idx, state, buf); - } - } - - display_write(gpio_display, 0, 0, &desc, buf); - k_sleep(K_MSEC(16)); - } -} - - -void spawn_gpio_visualizer_thread(void) { - k_thread_create(&gpio_visualizer_thread_data, gpio_visualizer__thread_stack, - GPIO_VISUALIZER_STACK_SIZE, - gpio_visualizer_thread, - NULL, NULL, NULL, - GPIO_VISUALIZER_PRIORITY, 0, K_NO_WAIT); -} \ No newline at end of file diff --git a/src/emulation/gpio_visualizer.hpp b/src/emulation/gpio_visualizer.hpp deleted file mode 100644 index 903e277..0000000 --- a/src/emulation/gpio_visualizer.hpp +++ /dev/null @@ -1 +0,0 @@ -void spawn_gpio_visualizer_thread(void); \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 3fe45fb..dcefc11 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,32 +1,95 @@ -#include -#include -#include -#include -#include +#include -// IP and port of the PC running XRCE-DDS agent -#define AGENT_IP "192.168.1.100" -#define AGENT_PORT 8888 +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +#include +#include + +#include +#include + +#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);for(;;){};}} +#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; +std_msgs__msg__Int32 msg; + +void timer_callback(rcl_timer_t * timer, int64_t last_call_time) +{ + RCLC_UNUSED(last_call_time); + if (timer != NULL) { + RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); + printk("msg: %d", msg.data++); + } +} int main(void) { - // Initialize micro-ROS support for Zephyr UDP transport - rmw_uros_set_custom_transport( - true, // enable - NULL, // user data (none) - zephyr_transport_open, - zephyr_transport_close, - zephyr_transport_write, - zephyr_transport_read); + rmw_uros_set_custom_transport( + MICRO_ROS_FRAMING_REQUIRED, + (void *) &default_params, + zephyr_transport_open, + zephyr_transport_close, + zephyr_transport_write, + zephyr_transport_read + ); - // Initialize rcl context, node, publisher, subscriber, etc... + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; - // Your app code here + // create init_options + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - while (1) { - // Spin or sleep - k_sleep(K_MSEC(100)); - } + // create node + rcl_node_t node; + RCCHECK(rclc_node_init_default(&node, "zephyr_int32_publisher", "", &support)); - return 0; -} + // create publisher + RCCHECK(rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + "zephyr_int32_publisher")); + + // create timer, + rcl_timer_t 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; + RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); + RCCHECK(rclc_executor_add_timer(&executor, &timer)); + + msg.data = 0; + + while(1){ + rclc_executor_spin_some(&executor, 100); + usleep(100000); + } + + // free resources + RCCHECK(rcl_publisher_fini(&publisher, &node)) + RCCHECK(rcl_node_fini(&node)) + return 0; +} \ No newline at end of file