SHERPA/microros_turntable_controller
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

working implementation

Browse Source
This commit is contained in:
Timo Schneider
2025-08-22 16:20:41 +02:00
parent ef5035b92b
commit 0bb1b5d026
26 changed files with 1104 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
modules/libmicroros/.gitignore vendored Normal file
View 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
View 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
View 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
View 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
View 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;

168
modules/libmicroros/microros_transports/serial-usb/microros_transports.c Normal file
View File

@@ -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;
}

49
modules/libmicroros/microros_transports/serial-usb/microros_transports.h Normal file
View File

@@ -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_

123
modules/libmicroros/microros_transports/serial/microros_transports.c Normal file
View File

@@ -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;
}

49
modules/libmicroros/microros_transports/serial/microros_transports.h Normal file
View File

@@ -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_

103
modules/libmicroros/microros_transports/udp/microros_transports.c Normal file
View File

@@ -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(&params->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;
}

47
modules/libmicroros/microros_transports/udp/microros_transports.h Normal file
View File

@@ -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
View 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
View File

@@ -0,0 +1,3 @@
build:
cmake: .
kconfig: Kconfig

34
modules/libmicroros/zephyr_toolchain.cmake.in Normal file
View 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)

1
modules/micro_ros_zephyr_module

Submodule modules/micro_ros_zephyr_module deleted from d1e19be708