Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
61 changes: 60 additions & 1 deletion zenbedded_transport/Kconfig
Original file line number Diff line number Diff line change
Expand Up @@ -122,9 +122,68 @@ config ZENBEDDED_NODE_NAME
The unique identifier for this specific microcontroller on the network.

# ==============================================================================
# TIER 2 CONFIGURATION
# BUFFER & POOL SIZING CONFIGURATION
# ==============================================================================

config ZENBEDDED_MAX_NODE_NAME_LEN
int "Maximum Node Name Length"
default 64
range 16 256
help
Maximum length of the ROS 2 node name string buffer (including null terminator).

config ZENBEDDED_MAX_TOPIC_NAME_LEN
int "Maximum Topic Name Length"
default 64
range 16 256
help
Maximum length of topic name strings and mangled topic buffers (including null terminator).
Comment thread
EnejiOhieku marked this conversation as resolved.

config ZENBEDDED_MAX_TYPE_NAME_LEN
int "Maximum DDS Type Name Length"
depends on ZENBEDDED_TIER_1
default 64
range 16 256
help
Maximum length of ROS 2 DDS type name strings (including null terminator, e.g. sensor_msgs::msg::dds_::JointState_).

config ZENBEDDED_MAX_LOCATOR_LEN
int "Maximum Locator String Length"
default 32
range 32 512
help
Maximum length of Zenoh locator strings (e.g. tcp/127.0.0.1:7447).

config ZENBEDDED_MAX_PUBLISHERS
int "Maximum Number of Publishers"
default 10
range 0 64
help
Maximum number of concurrent publishers supported by the transport pool.

config ZENBEDDED_MAX_SUBSCRIBERS
int "Maximum Number of Subscribers"
default 10
range 0 64
help
Maximum number of concurrent subscribers supported by the transport pool.
Comment thread
EnejiOhieku marked this conversation as resolved.

config ZENBEDDED_TIER1_MAX_RX_BUFFER_SIZE
int "Tier 1 Maximum RX Buffer Size"
depends on ZENBEDDED_TIER_1
default 1024
range 64 4096
help
Maximum payload buffer size in bytes for Tier 1 subscriber reception (Serialized ROS2 message size).

config ZENBEDDED_TIER2_MAX_RX_BUFFER_SIZE
int "Tier 2 Maximum RX Buffer Size"
depends on ZENBEDDED_TIER_2
default 64
range 8 512
help
Maximum payload buffer size in bytes for Tier 2 subscriber reception (size of the generated tier2 state struct).

config ZENBEDDED_TIER2_SCHEMA_PATH
string "Path to Tier 2 YAML Schema"
depends on ZENBEDDED_TIER_2
Expand Down
80 changes: 52 additions & 28 deletions zenbedded_transport/src/zenoh_transport.c
Original file line number Diff line number Diff line change
Expand Up @@ -19,14 +19,41 @@ LOG_MODULE_REGISTER(zenoh_transport, LOG_LEVEL_INF);

static z_owned_session_t z_session;
static uint32_t current_domain_id = 0;
static char current_node_name[64] = {0};
static char current_node_name[CONFIG_ZENBEDDED_MAX_NODE_NAME_LEN] = {0};
static bool is_initialized = false;

#define MAX_PUBLISHERS 10
#define MAX_SUBSCRIBERS 10
#define KEYEXPR_MAX_LEN 512
#define MAX_PUBLISHERS CONFIG_ZENBEDDED_MAX_PUBLISHERS
#define MAX_SUBSCRIBERS CONFIG_ZENBEDDED_MAX_SUBSCRIBERS
#define RMW_GID_SIZE 16

#if defined(CONFIG_ZENBEDDED_TIER_1)
/* ROS 2 / rmw_zenoh Key Expression and Liveliness Overhead Constants */
#define LV_NODE_FIXED_OVERHEAD 54 /* "@ros2_lv//<32-hex-zid>/0/0/NN/%/%/" */
#define LV_PUB_SUB_FIXED_OVERHEAD \
71 /* "@ros2_lv//<32-hex-zid>/0/10/{MP,MS}/%/%//%///::,7:,:,:,," */
#define TOPIC_KEYEXPR_FIXED_OVERHEAD \
3 /* Three '/' separators: "<domain_id>/<topic>/<type>/<hash>" */
#define MAX_DOMAIN_ID_DIGITS 10 /* Full uint32_t range (ROS 2 standard 0-232 uses 3 digits) */
#define RIHS01_TYPE_HASH_LEN 71 /* "RIHS01_" (7) + 64 hex chars */

#define NODE_LV_KEYEXPR_MAX_LEN \
(LV_NODE_FIXED_OVERHEAD + MAX_DOMAIN_ID_DIGITS + CONFIG_ZENBEDDED_MAX_NODE_NAME_LEN + 1)

#define TOPIC_KEYEXPR_MAX_LEN \
(TOPIC_KEYEXPR_FIXED_OVERHEAD + MAX_DOMAIN_ID_DIGITS + CONFIG_ZENBEDDED_MAX_TOPIC_NAME_LEN + \
CONFIG_ZENBEDDED_MAX_TYPE_NAME_LEN + RIHS01_TYPE_HASH_LEN + 1)

#define LV_KEYEXPR_MAX_LEN \
(LV_PUB_SUB_FIXED_OVERHEAD + MAX_DOMAIN_ID_DIGITS + CONFIG_ZENBEDDED_MAX_NODE_NAME_LEN + \
CONFIG_ZENBEDDED_MAX_TOPIC_NAME_LEN + CONFIG_ZENBEDDED_MAX_TYPE_NAME_LEN + \
RIHS01_TYPE_HASH_LEN + 1)

#elif defined(CONFIG_ZENBEDDED_TIER_2)

#define TOPIC_KEYEXPR_MAX_LEN CONFIG_ZENBEDDED_MAX_TOPIC_NAME_LEN

#endif

#if defined(CONFIG_ZENBEDDED_TIER_1)
typedef struct __attribute__((packed))
{
Expand All @@ -41,11 +68,11 @@ struct zenbedded_pub_s
{
bool in_use;
z_owned_publisher_t z_pub;
char topic_keyexpr[KEYEXPR_MAX_LEN];
char topic_keyexpr[TOPIC_KEYEXPR_MAX_LEN];
#if defined(CONFIG_ZENBEDDED_TIER_1)
rmw_zenoh_attachment_t attachment;
z_owned_liveliness_token_t lv_token;
char lv_keyexpr[KEYEXPR_MAX_LEN];
char lv_keyexpr[LV_KEYEXPR_MAX_LEN];
#endif
};

Expand All @@ -55,10 +82,10 @@ struct zenbedded_sub_s
z_owned_subscriber_t z_sub;
zenbedded_recv_cb_t user_cb;
void * user_data;
char topic_keyexpr[KEYEXPR_MAX_LEN];
char topic_keyexpr[TOPIC_KEYEXPR_MAX_LEN];
#if defined(CONFIG_ZENBEDDED_TIER_1)
z_owned_liveliness_token_t lv_token;
char lv_keyexpr[KEYEXPR_MAX_LEN];
char lv_keyexpr[LV_KEYEXPR_MAX_LEN];
#endif
};

Expand All @@ -67,7 +94,7 @@ static struct zenbedded_sub_s sub_pool[MAX_SUBSCRIBERS] = {0};

#if defined(CONFIG_ZENBEDDED_TIER_1)
static z_owned_liveliness_token_t node_lv_token;
static char node_lv_str[KEYEXPR_MAX_LEN] = {0};
static char node_lv_str[NODE_LV_KEYEXPR_MAX_LEN] = {0};
#endif

static void internal_sub_handler(z_loaned_sample_t * sample, void * arg)
Expand All @@ -82,7 +109,7 @@ static void internal_sub_handler(z_loaned_sample_t * sample, void * arg)
size_t len = z_bytes_len(payload);

#if defined(CONFIG_ZENBEDDED_TIER_1)
uint8_t rx_buf[1024];
static uint8_t rx_buf[CONFIG_ZENBEDDED_TIER1_MAX_RX_BUFFER_SIZE];
if (len == 0 || len > sizeof(rx_buf))
{
return;
Expand All @@ -93,21 +120,17 @@ static void internal_sub_handler(z_loaned_sample_t * sample, void * arg)
sub->user_cb(rx_buf, len, sub->user_data);

#elif defined(CONFIG_ZENBEDDED_TIER_2)
#define T2_MAX_BUFFER_BOUND 64

if (len == 0 || len > T2_MAX_BUFFER_BOUND)
if (len == 0 || len > CONFIG_ZENBEDDED_TIER2_MAX_RX_BUFFER_SIZE)
{
return;
}

uint8_t t2_rx_buf[T2_MAX_BUFFER_BOUND];
static uint8_t t2_rx_buf[CONFIG_ZENBEDDED_TIER2_MAX_RX_BUFFER_SIZE];

z_bytes_reader_t reader = z_bytes_get_reader(payload);
z_bytes_reader_read(&reader, t2_rx_buf, len);

sub->user_cb(t2_rx_buf, len, sub->user_data);

#undef T2_MAX_BUFFER_BOUND
#endif
}

Expand All @@ -121,12 +144,13 @@ int zenbedded_transport_init(

current_domain_id = domain_id;
strncpy(current_node_name, node_name, sizeof(current_node_name) - 1);
current_node_name[sizeof(current_node_name) - 1] = '\0';

z_owned_config_t z_config;
z_config_default(&z_config);
zp_config_insert(z_config_loan_mut(&z_config), Z_CONFIG_MODE_KEY, mode);

static char full_locator[128];
static char full_locator[CONFIG_ZENBEDDED_MAX_LOCATOR_LEN];

if (locator && strlen(locator) > 0)
{
Expand Down Expand Up @@ -166,7 +190,7 @@ int zenbedded_transport_init(
z_id_t zid = z_info_zid(z_session_loan(&z_session));

snprintf(
node_lv_str, KEYEXPR_MAX_LEN,
node_lv_str, sizeof(node_lv_str),
"@ros2_lv/%u/%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/0/0/NN/%%/%%/%s",
current_domain_id, zid.id[0], zid.id[1], zid.id[2], zid.id[3], zid.id[4], zid.id[5], zid.id[6],
zid.id[7], zid.id[8], zid.id[9], zid.id[10], zid.id[11], zid.id[12], zid.id[13], zid.id[14],
Expand Down Expand Up @@ -292,8 +316,8 @@ zenbedded_pub_t zenbedded_transport_declare_publisher(
return NULL;
}
snprintf(
pub->topic_keyexpr, KEYEXPR_MAX_LEN, "%u/%s/%s/%s", current_domain_id, clean_topic, type_name,
type_hash);
pub->topic_keyexpr, sizeof(pub->topic_keyexpr), "%u/%s/%s/%s", current_domain_id, clean_topic,
type_name, type_hash);
z_view_keyexpr_from_str_unchecked(&ke, pub->topic_keyexpr);

if (
Expand All @@ -304,7 +328,7 @@ zenbedded_pub_t zenbedded_transport_declare_publisher(
return NULL;
}
#elif defined(CONFIG_ZENBEDDED_TIER_2)
snprintf(pub->topic_keyexpr, KEYEXPR_MAX_LEN, "%s", clean_topic);
snprintf(pub->topic_keyexpr, sizeof(pub->topic_keyexpr), "%s", clean_topic);
z_view_keyexpr_from_str_unchecked(&ke, pub->topic_keyexpr);

z_publisher_options_t pub_opts;
Expand All @@ -323,7 +347,7 @@ zenbedded_pub_t zenbedded_transport_declare_publisher(
#endif

#if defined(CONFIG_ZENBEDDED_TIER_1)
char mangled_topic[64];
char mangled_topic[CONFIG_ZENBEDDED_MAX_TOPIC_NAME_LEN];
mangle_topic(mangled_topic, sizeof(mangled_topic), clean_topic);

pub->attachment.sequence_number = 0;
Expand All @@ -332,7 +356,7 @@ zenbedded_pub_t zenbedded_transport_declare_publisher(

z_id_t zid = z_info_zid(z_session_loan(&z_session));
snprintf(
pub->lv_keyexpr, KEYEXPR_MAX_LEN,
pub->lv_keyexpr, sizeof(pub->lv_keyexpr),
"@ros2_lv/%u/%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/0/10/MP/%%/%%/%s/"
"%%%s/%s/%s/::,7:,:,:,,",
current_domain_id, zid.id[0], zid.id[1], zid.id[2], zid.id[3], zid.id[4], zid.id[5], zid.id[6],
Expand Down Expand Up @@ -435,10 +459,10 @@ zenbedded_sub_t zenbedded_transport_declare_subscriber(
return NULL;
}
snprintf(
sub->topic_keyexpr, KEYEXPR_MAX_LEN, "%u/%s/%s/%s", current_domain_id, clean_topic, type_name,
type_hash);
sub->topic_keyexpr, sizeof(sub->topic_keyexpr), "%u/%s/%s/%s", current_domain_id, clean_topic,
type_name, type_hash);
#elif defined(CONFIG_ZENBEDDED_TIER_2)
snprintf(sub->topic_keyexpr, KEYEXPR_MAX_LEN, "%s", clean_topic);
snprintf(sub->topic_keyexpr, sizeof(sub->topic_keyexpr), "%s", clean_topic);
#endif

z_view_keyexpr_t ke;
Expand Down Expand Up @@ -470,12 +494,12 @@ zenbedded_sub_t zenbedded_transport_declare_subscriber(
#endif

#if defined(CONFIG_ZENBEDDED_TIER_1)
char mangled_topic[64];
char mangled_topic[CONFIG_ZENBEDDED_MAX_TOPIC_NAME_LEN];
mangle_topic(mangled_topic, sizeof(mangled_topic), clean_topic);

z_id_t zid = z_info_zid(z_session_loan(&z_session));
snprintf(
sub->lv_keyexpr, KEYEXPR_MAX_LEN,
sub->lv_keyexpr, sizeof(sub->lv_keyexpr),
"@ros2_lv/%u/%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/0/10/MS/%%/%%/%s/"
"%%%s/%s/%s/::,7:,:,:,,",
current_domain_id, zid.id[0], zid.id[1], zid.id[2], zid.id[3], zid.id[4], zid.id[5], zid.id[6],
Expand Down
Loading