MicroROS zephyr demo

Control.srv

uint8 module # module to control
uint8 command # value to set
---
bool success   # indicate successful run of triggered service

main.c

#include <zephyr.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rcl/error_handling.h>
#include <rclc_parameter/rclc_parameter.h>

#include <std_msgs/msg/u_int16.h>
#include <gardin_interfaces/msg/servos.h>
#include <gardin_interfaces/srv/control.h>

#include <rmw_microros/rmw_microros.h>
#include <microros_transports.h>

#include "app_version.h"
#include "board.h"

#include <logging/log.h>
LOG_MODULE_REGISTER(main, CONFIG_APP_LOG_LEVEL);

rcl_node_t node;
rclc_executor_t executor;
rclc_parameter_server_t param_server;
rcl_subscription_t servo_subscription;
gardin_interfaces__msg__Servos servo_msg;

void on_parameter_changed(Parameter * param)
{
	if (strcmp(param->name.data, "trigger_delay_us") == 0 && param->value.type == RCLC_PARAMETER_INT)
	{
		//publish = param->value.bool_value;
		//LOG_INF("Publish %s", (publish) ? "ON" : "OFF");
		LOG_INF("Trigger delayed changed to %d", (uint16_t)param->value.integer_value);
	}
	else if (strcmp(param->name.data, "laser_pulse_us") == 0 && param->value.type == RCLC_PARAMETER_INT)
	{
		LOG_INF("Laser pulse width changed to %d", (uint16_t)param->value.integer_value);
	}
}

void servo_subscription_callback(const void * msgin)
{
	const gardin_interfaces__msg__Servos * msg = (const gardin_interfaces__msg__Servos *)msgin;
	LOG_INF("Moving Servo %d to %d", msg->id, msg->angle);
	set_servo_angle(msg->id, msg->angle);
}

void control_service_callback(const void * req, void * res)
{
	// int err;
	gardin_interfaces__srv__Control_Request * req_in = (gardin_interfaces__srv__Control_Request *) req;
	gardin_interfaces__srv__Control_Response * res_in = (gardin_interfaces__srv__Control_Response *) res;

	LOG_INF("Service request value: %d: %d.", req_in->module, req_in->command);

	// err = set_gpio_state(req_in->module, req_in->command);
	// res_in->success = (err == 0);
	res_in->success = 1;
}
/*
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
	RCLC_UNUSED(last_call_time);
	if (timer != NULL) {
		struct sensor_value value;
		std_msgs__msg__Float32 msg;

		sensor_sample_fetch(dev);
		sensor_channel_get(dev, SENSOR_CHAN_DISTANCE, &value);
		msg.data= ((float)value.val1 + (float)value.val2)/1000.0;

		rcl_publish(&tof_publisher, (const void*)&msg, NULL);
	}
}
*/
void main(void)
{
	int err;
	rcl_ret_t rc;

	LOG_INF("uROS Application %s\n", APP_VERSION_STR);

	err = board_init();
	if (err) {
		LOG_ERR("board init failed (err %d)\n", err);
		return;
	}

	rmw_uros_set_custom_transport(
		MICRO_ROS_FRAMING_REQUIRED,
		(void *) &default_params,
		zephyr_transport_open,
		zephyr_transport_close,
		zephyr_transport_write,
		zephyr_transport_read
	);

	rcl_allocator_t allocator = rcl_get_default_allocator();

	rclc_support_t support;
	rc = rclc_support_init(&support, 0, NULL, &allocator);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to init support (%d)", rc);
		return;
	}

	rc = rclc_node_init_default(&node, "gardin_ec", "", &support);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to create node (%d)", rc);
		goto error;
	}

	// Max params 4 by default. Do init_with_options
	rc = rclc_parameter_server_init_default(&param_server, &node);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to create parameter server (%d)", rc);
	}

	rcl_service_t service;
	rc = rclc_service_init_default(
		&service,
		&node,
		ROSIDL_GET_SRV_TYPE_SUPPORT(gardin_interfaces, srv, Control),
		"/control");
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to create control service (%d)", rc);
		goto error;
	}

	rc = rclc_subscription_init_default(
		&servo_subscription,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(gardin_interfaces, msg, Servos),
		"/servo");
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to create servo subscription (%d)", rc);
		goto error;
	}
/*
	rcl_timer_t timer;
	rc = rclc_timer_init_default(&timer, &support, RCL_S_TO_NS(10), timer_callback);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to create timer (%d)", rc);
		goto error;
	}
*/
	// Parameter server has 5 services and 1 publisher */
	rc = rclc_executor_init(&executor, &support.context, 8, &allocator);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to init executor (%d)", rc);
		goto error;
	}

	rc = rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to init parameter service (%d)", rc);
		goto error;
	}

	gardin_interfaces__srv__Control_Request req;
	gardin_interfaces__srv__Control_Response res;
	rc = rclc_executor_add_service(&executor, &service, &req, &res, control_service_callback);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to init service (%d)", rc);
		goto error;
	}

	rc = rclc_executor_add_subscription(
		&executor,
		&servo_subscription,
		&servo_msg,
		&servo_subscription_callback,
		ON_NEW_DATA);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to init servo subscription (%d)", rc);
		goto error;
	}

	rclc_add_parameter(&param_server, "trigger_delay_us", RCLC_PARAMETER_INT);
	rclc_add_parameter(&param_server, "laser_pulse_us", RCLC_PARAMETER_INT);

	rclc_parameter_set_int(&param_server, "trigger_delay_us", 750);
	rclc_parameter_set_int(&param_server, "laser_pulse_us", 20);
/*
	rc = rclc_executor_add_timer(&executor, &timer);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed to add timer (%d)", rc);
		goto error;
	}
*/
	while(1) {
		rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
		usleep(10000);
	}

error:
	rc = rcl_service_fini(&service, &node);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed deinit service (%d)", rc);
	}

	rc = rcl_subscription_fini(&servo_subscription, &node);

	rc = rcl_node_fini(&node);
	if (rc != RCL_RET_OK) {
		LOG_ERR("Failed deinit node (%d)", rc);
	}
}

Servos.msg

uint8 id
uint8 angle