PX4-Autopilot/src/modules/zenoh/zenoh.cpp

/****************************************************************************
 *
 *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "zenoh.h"
#include "zenoh-pico/api/macros.h"
#include "zenoh-pico/api/primitives.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/cli.h>
#include <errno.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <ctype.h>
#include <string.h>

#include <zenoh-pico.h>

// CycloneDDS CDR Deserializer
#include <dds/cdr/dds_cdrstream.h>

// Auto-generated header to all uORB <-> CDR conversions
#include <uorb_pubsub_factory.hpp>

#define Z_PUBLISH
#define Z_SUBSCRIBE

extern "C" __EXPORT int zenoh_main(int argc, char *argv[]);

void toCamelCase(char *input)
{
	int capitalizeNext = 1; // Capitalize the first letter
	int j = 0;

	for (int i = 0; input[i] != '\0'; i++) {
		if (input[i] == '_') {
			capitalizeNext = 1; // Next letter should be capitalized

		} else {
			if (capitalizeNext && isalpha(input[i])) {
				input[j++] = toupper(input[i]);
				capitalizeNext = 0;

			} else {
				input[j++] = input[i];
			}
		}
	}

	input[j] = '\0'; // Null-terminate the input string
}

ZENOH::ZENOH():
	ModuleParams(nullptr)
{

}

ZENOH::~ZENOH()
{

}

void ZENOH::run()
{
	char mode[NET_MODE_SIZE];
	char locator[NET_LOCATOR_SIZE];
	int8_t ret;
	int i;

#ifndef BOARD_HAS_NO_UUID
	px4_guid_t px4_guid;
	board_get_px4_guid(px4_guid);
#else
	// TODO Fill ID with something reasonable
	px4_guid_t px4_guid = {0xAA, 0xBB, 0xAA};
#endif

	Zenoh_Config z_config;

	z_config.getNetworkConfig(mode, locator);

	z_owned_config_t config;
	z_config_default(&config);
	zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode);

	if (locator[0] != 0) {
		zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator);

	} else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) {
		zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT);
	}

	PX4_INFO("Opening session...");
	z_owned_session_t s;
	ret = z_open(&s, z_move(config), NULL);

	if (ret < 0) {
		if (ret == _Z_ERR_TRANSPORT_OPEN_FAILED) {
			PX4_ERR("Unable to open session, make sure zenohd is running on %s", locator);

		} else if (ret == _Z_ERR_SCOUT_NO_RESULTS) {
			PX4_ERR("Unable to open session, scout no results");

		} else {
			PX4_ERR("Unable to open session, ret: %d", ret);
		}

		return;
	}

	PX4_INFO("Starting reading/writing tasks...");

	// Start read and lease tasks for zenoh-pico
	if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) {
		PX4_ERR("Unable to start read and lease tasks");
		z_drop(z_move(s));
		return;
	}

#ifdef Z_SUBSCRIBE
	_sub_count =  z_config.getSubCount();
	_zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count);
	{
		char topic[TOPIC_INFO_SIZE];
		char type[TOPIC_INFO_SIZE];
		char keyexpr[KEYEXPR_SIZE];

		for (i = 0; i < _sub_count; i++) {
			z_config.getSubscriberMapping(topic, type);
			_zenoh_subscribers[i] = genSubscriber(type);

			if (_zenoh_subscribers[i] != 0) {
				const uint8_t *rihs_hash = getRIHS01_Hash(type);
				toCamelCase(type); // Convert uORB type to camel case
				snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%s/"
					 KEYEXPR_MSG_NAME "%s_/RIHS01_"
					 "%02x%02x%02x%02x%02x%02x%02x%02x"
					 "%02x%02x%02x%02x%02x%02x%02x%02x"
					 "%02x%02x%02x%02x%02x%02x%02x%02x"
					 "%02x%02x%02x%02x%02x%02x%02x%02x",
					 _zenoh_domain_id.get(), topic, type,
					 rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3],
					 rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7],
					 rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11],
					 rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15],
					 rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19],
					 rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23],
					 rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27],
					 rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31]
					);
				_zenoh_subscribers[i]->declare_subscriber(s, keyexpr);

			} else {
				PX4_ERR("Could not create a subscriber for type %s", type);
			}

		}

		if (z_config.getSubscriberMapping(topic, type) < 0) {
			PX4_WARN("Subscriber mapping parsing error");
		}
	}
#endif

#ifdef Z_PUBLISH

	_pub_count =  z_config.getPubCount();
	_zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *));
	px4_pollfd_struct_t pfds[_pub_count];

	{
		char topic[TOPIC_INFO_SIZE];
		char type[TOPIC_INFO_SIZE];
		char keyexpr[KEYEXPR_SIZE];

		for (i = 0; i < _pub_count; i++) {
			z_config.getPublisherMapping(topic, type);
			_zenoh_publishers[i] = genPublisher(type);

			if (_zenoh_publishers[i] != 0) {
				const uint8_t *rihs_hash = getRIHS01_Hash(type);
				toCamelCase(type); // Convert uORB type to camel case
				snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%s/"
					 KEYEXPR_MSG_NAME "%s_/RIHS01_"
					 "%02x%02x%02x%02x%02x%02x%02x%02x"
					 "%02x%02x%02x%02x%02x%02x%02x%02x"
					 "%02x%02x%02x%02x%02x%02x%02x%02x"
					 "%02x%02x%02x%02x%02x%02x%02x%02x",
					 _zenoh_domain_id.get(), topic, type,
					 rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3],
					 rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7],
					 rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11],
					 rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15],
					 rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19],
					 rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23],
					 rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27],
					 rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31]
					);
				_zenoh_publishers[i]->declare_publisher(s, keyexpr, (uint8_t *)&px4_guid);
				_zenoh_publishers[i]->setPollFD(&pfds[i]);

			} else {
				PX4_ERR("Could not create a publisher for type %s", type);
			}
		}

		if (z_config.getSubscriberMapping(topic, type) < 0) {
			PX4_WARN("Publisher mapping parsing error");
		}
	}

	if (_pub_count == 0) {
		// Nothing to publish but we don't want to stop this thread
		while (!should_exit()) {
			usleep(1000);
		}
	}

	while (!should_exit()) {
		int pret = px4_poll(pfds, _pub_count, 100);

		if (pret == 0) {
			//PX4_INFO("Zenoh poll timeout\n");

		} else {
			for (i = 0; i < _pub_count; i++) {
				if (pfds[i].revents & POLLIN) {
					ret = _zenoh_publishers[i]->update();

					if (ret < 0) {
						PX4_WARN("Publisher error %i", ret);

					}
				}
			}
		}
	}

#endif

	// Exiting cleaning up publisher and subscribers
	for (i = 0; i < _sub_count; i++) {
		if (_zenoh_subscribers[i]) {
			delete _zenoh_subscribers[i];
		}
	}

	free(_zenoh_subscribers);

	for (i = 0; i < _pub_count; i++) {
		if (_zenoh_publishers[i]) {
			delete _zenoh_publishers[i];
		}
	}

	free(_zenoh_publishers);

	// Stop read and lease tasks for zenoh-pico
	zp_stop_read_task(z_session_loan_mut(&s));
	zp_stop_lease_task(z_session_loan_mut(&s));

	z_drop(z_session_move(&s));
	exit_and_cleanup();
}

int ZENOH::custom_command(int argc, char *argv[])
{
	if (argc > 0 && strcmp("config", argv[0]) == 0) {
		Zenoh_Config z_config;

		if (z_config.cli(argc, argv) == 0) {
			return 0;
		}
	}

	return print_usage("Unrecognized command.");
}

int ZENOH::print_usage(const char *reason)
{
	if (reason) {
		printf("%s\n\n", reason);
	}

	PRINT_MODULE_USAGE_NAME("zenoh", "driver");
	PRINT_MODULE_DESCRIPTION(R"DESC_STR(
### Description

Zenoh demo bridge
	)DESC_STR");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_COMMAND("stop");
	PRINT_MODULE_USAGE_COMMAND("status");
	PRINT_MODULE_USAGE_COMMAND("config");
	PX4_INFO_RAW("     addpublisher  <zenoh_topic> <uorb_topic>  Publish uORB topic to Zenoh\n");
	PX4_INFO_RAW("     addsubscriber <zenoh_topic> <uorb_topic>  Publish Zenoh topic to uORB\n");
	PX4_INFO_RAW("     net           <mode> <locator>            Zenoh network mode\n");
	PX4_INFO_RAW("          <mode>    values: client|peer   \n");
	PX4_INFO_RAW("          <locator> client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n");
	PX4_INFO_RAW("                    peer: multicast address e.g. udp/224.0.0.224:7446#iface=eth0\n");
	return 0;
}

int ZENOH::print_status()
{
	PX4_INFO("running");

	PX4_INFO("Publishers");

	for (int i = 0; i < _pub_count; i++) {
		if (_zenoh_publishers[i]) {
			_zenoh_publishers[i]->print();
		}
	}

	PX4_INFO("Subscribers");

	for (int i = 0; i < _sub_count; i++) {
		if (_zenoh_subscribers[i]) {
			_zenoh_subscribers[i]->print();
		}
	}

	return 0;
}

int ZENOH::task_spawn(int argc, char *argv[])
{

	int task_id = px4_task_spawn_cmd(
			      "zenoh",
			      SCHED_DEFAULT,
			      SCHED_PRIORITY_DEFAULT,
			      4096,
			      &run_trampoline,
			      argv
		      );

	if (task_id < 0) {
		return -errno;

	} else {
		_task_id = task_id;
		return 0;
	}
}

ZENOH *ZENOH::instantiate(int argc, char *argv[])
{
	return new ZENOH();
}

int zenoh_main(int argc, char *argv[])
{
	return ZENOH::main(argc, argv);
}