working implementation
This commit is contained in:
@@ -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)
|
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})
|
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
||||||
|
|
||||||
project(sherpa_ros2_sensor_node)
|
project(sherpa_ros2_sensor_node)
|
||||||
|
|
||||||
target_sources(app PRIVATE src/main.cpp)
|
target_sources(app PRIVATE src/main.cpp)
|
||||||
|
|
||||||
if(${BOARD} STREQUAL "native_sim")
|
|
||||||
add_subdirectory(src/emulation)
|
|
||||||
endif()
|
|
||||||
|
|||||||
@@ -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"
|
|
||||||
@@ -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";
|
|
||||||
};
|
|
||||||
};
|
|
||||||
@@ -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"
|
|
||||||
@@ -10,4 +10,15 @@
|
|||||||
label = "USER_LED_0";
|
label = "USER_LED_0";
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
};
|
||||||
|
|
||||||
|
&uart4 {
|
||||||
|
status = "okay";
|
||||||
|
current-speed = <115200>;
|
||||||
|
pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>;
|
||||||
|
pinctrl-names = "default";
|
||||||
|
};
|
||||||
|
|
||||||
|
&usart3 {
|
||||||
|
status = "okay";
|
||||||
};
|
};
|
||||||
@@ -6,7 +6,6 @@ common:
|
|||||||
min_flash: 16
|
min_flash: 16
|
||||||
tags: microros
|
tags: microros
|
||||||
integration_platforms:
|
integration_platforms:
|
||||||
- native_sim
|
|
||||||
- nucleo_h563zi
|
- nucleo_h563zi
|
||||||
harness: console
|
harness: console
|
||||||
harness_config:
|
harness_config:
|
||||||
|
|||||||
6
modules/libmicroros/.gitignore
vendored
Normal file
6
modules/libmicroros/.gitignore
vendored
Normal file
@@ -0,0 +1,6 @@
|
|||||||
|
micro_ros_dev
|
||||||
|
micro_ros_src
|
||||||
|
zephyr_toolchain.cmake
|
||||||
|
include
|
||||||
|
configured_colcon.meta
|
||||||
|
*.a
|
||||||
107
modules/libmicroros/CMakeLists.txt
Normal file
107
modules/libmicroros/CMakeLists.txt
Normal file
@@ -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()
|
||||||
113
modules/libmicroros/Kconfig
Normal file
113
modules/libmicroros/Kconfig
Normal file
@@ -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
|
||||||
|
|
||||||
48
modules/libmicroros/colcon.meta
Normal file
48
modules/libmicroros/colcon.meta
Normal file
@@ -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"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
136
modules/libmicroros/libmicroros.mk
Normal file
136
modules/libmicroros/libmicroros.mk
Normal file
@@ -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;
|
||||||
@@ -0,0 +1,168 @@
|
|||||||
|
#include <uxr/client/transport.h>
|
||||||
|
|
||||||
|
#include <version.h>
|
||||||
|
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
|
||||||
|
#include <zephyr/kernel.h>
|
||||||
|
#include <zephyr/device.h>
|
||||||
|
#include <zephyr/sys/printk.h>
|
||||||
|
#include <zephyr/drivers/uart.h>
|
||||||
|
#include <zephyr/sys/ring_buffer.h>
|
||||||
|
#include <zephyr/usb/usb_device.h>
|
||||||
|
#include <zephyr/posix/unistd.h>
|
||||||
|
#else
|
||||||
|
#include <zephyr.h>
|
||||||
|
#include <device.h>
|
||||||
|
#include <sys/printk.h>
|
||||||
|
#include <drivers/uart.h>
|
||||||
|
#include <sys/ring_buffer.h>
|
||||||
|
#include <usb/usb_device.h>
|
||||||
|
#include <posix/unistd.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#include <microros_transports.h>
|
||||||
|
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
||||||
@@ -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 <unistd.h>
|
||||||
|
#include <version.h>
|
||||||
|
|
||||||
|
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
|
||||||
|
#include <zephyr/device.h>
|
||||||
|
#else
|
||||||
|
#include <device.h>
|
||||||
|
#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_
|
||||||
@@ -0,0 +1,123 @@
|
|||||||
|
#include <uxr/client/transport.h>
|
||||||
|
|
||||||
|
#include <microros_transports.h>
|
||||||
|
#include <version.h>
|
||||||
|
|
||||||
|
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
|
||||||
|
#include <zephyr/kernel.h>
|
||||||
|
#include <zephyr/device.h>
|
||||||
|
#include <zephyr/sys/printk.h>
|
||||||
|
#include <zephyr/drivers/uart.h>
|
||||||
|
#include <zephyr/sys/ring_buffer.h>
|
||||||
|
#include <zephyr/posix/unistd.h>
|
||||||
|
#else
|
||||||
|
#include <zephyr.h>
|
||||||
|
#include <device.h>
|
||||||
|
#include <sys/printk.h>
|
||||||
|
#include <drivers/uart.h>
|
||||||
|
#include <sys/ring_buffer.h>
|
||||||
|
#include <posix/unistd.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
||||||
@@ -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 <unistd.h>
|
||||||
|
#include <version.h>
|
||||||
|
|
||||||
|
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
|
||||||
|
#include <zephyr/device.h>
|
||||||
|
#else
|
||||||
|
#include <device.h>
|
||||||
|
#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_
|
||||||
@@ -0,0 +1,103 @@
|
|||||||
|
#include <uxr/client/transport.h>
|
||||||
|
|
||||||
|
#include <version.h>
|
||||||
|
|
||||||
|
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
|
||||||
|
#include <zephyr/kernel.h>
|
||||||
|
#include <zephyr/posix/unistd.h>
|
||||||
|
#include <zephyr/posix/arpa/inet.h>
|
||||||
|
#include <zephyr/posix/netdb.h>
|
||||||
|
#else
|
||||||
|
#include <zephyr.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <netdb.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <errno.h>
|
||||||
|
|
||||||
|
#include <microros_transports.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
@@ -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 <unistd.h>
|
||||||
|
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <posix/sys/socket.h>
|
||||||
|
#include <posix/poll.h>
|
||||||
|
|
||||||
|
#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_
|
||||||
14
modules/libmicroros/utils.sh
Normal file
14
modules/libmicroros/utils.sh
Normal file
@@ -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"; \
|
||||||
|
}
|
||||||
3
modules/libmicroros/zephyr/module.yml
Normal file
3
modules/libmicroros/zephyr/module.yml
Normal file
@@ -0,0 +1,3 @@
|
|||||||
|
build:
|
||||||
|
cmake: .
|
||||||
|
kconfig: Kconfig
|
||||||
34
modules/libmicroros/zephyr_toolchain.cmake.in
Normal file
34
modules/libmicroros/zephyr_toolchain.cmake.in
Normal file
@@ -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)
|
||||||
Submodule modules/micro_ros_zephyr_module deleted from d1e19be708
21
prj.conf
21
prj.conf
@@ -1,4 +1,5 @@
|
|||||||
# c version
|
# c version
|
||||||
|
CONFIG_MAIN_STACK_SIZE=8192
|
||||||
CONFIG_CPP=y
|
CONFIG_CPP=y
|
||||||
CONFIG_NEWLIB_LIBC=y
|
CONFIG_NEWLIB_LIBC=y
|
||||||
CONFIG_NEWLIB_LIBC_NANO=n
|
CONFIG_NEWLIB_LIBC_NANO=n
|
||||||
@@ -7,27 +8,15 @@ CONFIG_MINIMAL_LIBC=n
|
|||||||
CONFIG_POSIX_API=y
|
CONFIG_POSIX_API=y
|
||||||
|
|
||||||
# logging
|
# logging
|
||||||
CONFIG_LOG=n
|
CONFIG_LOG=y
|
||||||
|
|
||||||
# gpio support
|
# gpio support
|
||||||
CONFIG_GPIO=y
|
CONFIG_GPIO=y
|
||||||
|
|
||||||
# display for native_sim
|
# usb
|
||||||
CONFIG_DISPLAY=y
|
CONFIG_UART_INTERRUPT_DRIVEN=y
|
||||||
CONFIG_SDL_DISPLAY=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
|
# micro ros
|
||||||
CONFIG_MICROROS=y
|
CONFIG_MICROROS=y
|
||||||
CONFIG_MICROROS_TRANSPORT_UDP=y
|
CONFIG_MICROROS_TRANSPORT_SERIAL=y
|
||||||
@@ -1,3 +0,0 @@
|
|||||||
target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
|
|
||||||
target_sources(app PRIVATE gpio_visualizer.cpp)
|
|
||||||
|
|
||||||
@@ -1,89 +0,0 @@
|
|||||||
#include "gpio_visualizer.hpp"
|
|
||||||
|
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
#include <zephyr/device.h>
|
|
||||||
#include <zephyr/sys/printk.h>
|
|
||||||
#include <zephyr/devicetree.h>
|
|
||||||
#include <zephyr/drivers/gpio/gpio_emul.h>
|
|
||||||
#include <zephyr/drivers/display.h>
|
|
||||||
|
|
||||||
#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);
|
|
||||||
}
|
|
||||||
@@ -1 +0,0 @@
|
|||||||
void spawn_gpio_visualizer_thread(void);
|
|
||||||
111
src/main.cpp
111
src/main.cpp
@@ -1,32 +1,95 @@
|
|||||||
#include <rcl/rcl.h>
|
#include <version.h>
|
||||||
#include <rclc/rclc.h>
|
|
||||||
#include <uxr/client/config.h>
|
|
||||||
#include <uxr/client/client.h>
|
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
|
|
||||||
// IP and port of the PC running XRCE-DDS agent
|
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
|
||||||
#define AGENT_IP "192.168.1.100"
|
#include <zephyr/kernel.h>
|
||||||
#define AGENT_PORT 8888
|
#include <zephyr/device.h>
|
||||||
|
#include <zephyr/devicetree.h>
|
||||||
|
#include <zephyr/drivers/gpio.h>
|
||||||
|
#include <zephyr/posix/time.h>
|
||||||
|
#else
|
||||||
|
#include <zephyr.h>
|
||||||
|
#include <device.h>
|
||||||
|
#include <devicetree.h>
|
||||||
|
#include <drivers/gpio.h>
|
||||||
|
#include <posix/time.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <rcl/rcl.h>
|
||||||
|
#include <rcl/error_handling.h>
|
||||||
|
#include <std_msgs/msg/int32.h>
|
||||||
|
|
||||||
|
#include <rclc/rclc.h>
|
||||||
|
#include <rclc/executor.h>
|
||||||
|
|
||||||
|
#include <rmw_microros/rmw_microros.h>
|
||||||
|
#include <microros_transports.h>
|
||||||
|
|
||||||
|
#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)
|
int main(void)
|
||||||
{
|
{
|
||||||
// Initialize micro-ROS support for Zephyr UDP transport
|
rmw_uros_set_custom_transport(
|
||||||
rmw_uros_set_custom_transport(
|
MICRO_ROS_FRAMING_REQUIRED,
|
||||||
true, // enable
|
(void *) &default_params,
|
||||||
NULL, // user data (none)
|
zephyr_transport_open,
|
||||||
zephyr_transport_open,
|
zephyr_transport_close,
|
||||||
zephyr_transport_close,
|
zephyr_transport_write,
|
||||||
zephyr_transport_write,
|
zephyr_transport_read
|
||||||
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) {
|
// create node
|
||||||
// Spin or sleep
|
rcl_node_t node;
|
||||||
k_sleep(K_MSEC(100));
|
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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user