micro ros parameters not working

[AbdullahAhmedTaman]

Issue template

• Hardware description: ESP32 dev module
• RTOS:
• Installation type: Arduino IDE
• Version or commit hash: Humble

Steps to reproduce the issue

Expected behavior

the micro ros parameter works

Actual behavior

the parameters doesn't appear in the parameter list in the terminal and the node is not working

Additional information

I was trying to make 16 parameters for a thrusters node (8 parameters for ratios and 8 for direction) but when i run the node the node it doesn't do any thing, it doesn't give me an error and the parameters doesn't show in the parameters list.

here is the full code:

#include <micro_ros_arduino.h>
#include <ESP32Servo.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc_parameter/rclc_parameter.h>
#include <rclc/executor.h>
#include <rcl/error_handling.h>
#include <geometry_msgs/msg/twist.h>
#include <std_msgs/msg/int64.h>  // Include message type for speed
#include <std_msgs/msg/int32_multi_array.h>  // Include message type for thrusters feedback
//#include <string.h>
#include <stdio.h>
#include <unistd.h>

rcl_subscription_t twist_subscriber;
rcl_subscription_t speed_subscriber;  // Subscriber for speed messages

rclc_parameter_server_t param_server;
// Setup parameter server options 
const rclc_parameter_options_t param_options = {
    .notify_changed_over_dds = true,
    .max_params = 16,
    .allow_undeclared_parameters = false,
    .low_mem_mode = false}; // to set all 16 once

rcl_node_t node;
rcl_allocator_t allocator;
rclc_support_t support;
rclc_executor_t executor;

// Timer for publishing thrusters feedback and for cheking parameters values
rcl_timer_t feedback_timer;

// Publisher for thrusters feedback
rcl_publisher_t thrusters_feedback_publisher;

std_msgs__msg__Int64 speed_msg;
geometry_msgs__msg__Twist twist_msg;
std_msgs__msg__Int32MultiArray thrusters_feedback_msg;

// Thrusters Pins
#define H_FRONT_LEFT_PIN    19
#define H_FRONT_RIGHT_PIN   18
#define H_BACK_LEFT_PIN     5     
#define H_BACK_RIGHT_PIN    17
     
#define V_FRONT_LEFT_PIN    16
#define V_FRONT_RIGHT_PIN   4
#define V_BACK_LEFT_PIN     2   
#define V_BACK_RIGHT_PIN    23

// Thrusters Numbers
#define H_F_L 0
#define H_F_R 1
#define H_B_L 2
#define H_B_R 3
#define V_F_L 4
#define V_F_R 5
#define V_B_L 6
#define V_B_R 7

// Define neutral and PWM limits
#define NEUTRAL_PWM         1500
#define MAX_FORWARD_PWM     2000
#define MAX_BACKWARD_PWM    1000
#define PWM_INCREMENT       100

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

// Define the thrusters and their PWM control
Servo H_FRONT_LEFT;
Servo H_FRONT_RIGHT;
Servo H_BACK_LEFT;
Servo H_BACK_RIGHT;
Servo V_FRONT_LEFT;
Servo V_FRONT_RIGHT;
Servo V_BACK_LEFT;
Servo V_BACK_RIGHT;

double thrust_ratios[8] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};  // Default ratios
int64_t thrust_directions[8] = {1, 1, 1, 1, 1, 1, 1, 1};  // Default directions (1 for forward, -1 for reverse)
int32_t thrusters_feedback_data[8]; // Arry to store thruster pwm values for feedback

// Variables to store the current PWM signals
int pwm_values[8] = {NEUTRAL_PWM, NEUTRAL_PWM, NEUTRAL_PWM, NEUTRAL_PWM, 
                     NEUTRAL_PWM, NEUTRAL_PWM, NEUTRAL_PWM, NEUTRAL_PWM};

// Speed multiplier (default to 2)
int64_t speed_multiplier = 2;  // Default speed multiplier

// set the variables for default values
double param_val_d = 1.0;
int64_t param_val_i = 1;


void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

// Clamp the PWM values within the ESC's range
int clamp_pwm(int value) {
    if (value < MAX_BACKWARD_PWM) return MAX_BACKWARD_PWM;
    if (value > MAX_FORWARD_PWM) return MAX_FORWARD_PWM;
    return value;
}

// Function to initialize thrusters
void setup_thrusters() {
    H_FRONT_LEFT.attach(H_FRONT_LEFT_PIN);
    H_FRONT_RIGHT.attach(H_FRONT_RIGHT_PIN);
    H_BACK_LEFT.attach(H_BACK_LEFT_PIN);
    H_BACK_RIGHT.attach(H_BACK_RIGHT_PIN);
    V_FRONT_LEFT.attach(V_FRONT_LEFT_PIN);
    V_FRONT_RIGHT.attach(V_FRONT_RIGHT_PIN);
    V_BACK_LEFT.attach(V_BACK_LEFT_PIN);
    V_BACK_RIGHT.attach(V_BACK_RIGHT_PIN);

    // Initialize all thrusters to neutral
    for (int i = 0; i < 8; i++) {
        pwm_values[i] = NEUTRAL_PWM;
    }
    apply_pwm();
}

void setup_parameters() {

  // Add parameters to the server
  // ratios
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_front_left", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_front_right", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_back_left", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_back_right", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_v_front_left", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_v_front_right", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_v_back_left", RCLC_PARAMETER_DOUBLE));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_ratio_v_back_right", RCLC_PARAMETER_DOUBLE));

  // direction
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_front_left", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_front_right", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_back_left", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_back_right", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_v_front_left", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_v_front_right", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_v_back_left", RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, "thrust_direction_v_back_right", RCLC_PARAMETER_INT));
  
  // Set parameter default values
  //ratios
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_front_left", thrust_ratios[H_F_L]));
  RCCHECK(rclc_parameter_set_double(&param_server,"thrust_ratio_front_right" , thrust_ratios[H_F_R]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_back_left", thrust_ratios[H_B_L]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_back_right", thrust_ratios[H_B_R]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_v_front_left", thrust_ratios[V_F_L]));
  RCCHECK(rclc_parameter_set_double(&param_server,"thrust_ratio_v_front_right" , thrust_ratios[V_F_R]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_v_back_left", thrust_ratios[V_B_L]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_v_back_right", thrust_ratios[V_B_R]));

  //directions
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_front_left", thrust_directions[H_F_L]));
  RCCHECK(rclc_parameter_set_int(&param_server,"thrust_direction_front_right" , thrust_directions[H_F_R]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_back_left", thrust_directions[H_B_L]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_back_right", thrust_directions[H_B_R]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_v_front_left", thrust_directions[V_F_L]));
  RCCHECK(rclc_parameter_set_int(&param_server,"thrust_direction_v_front_right" , thrust_directions[V_F_R]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_v_back_left", thrust_directions[V_B_L]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_v_back_right", thrust_directions[V_B_R]));

  // Get paramter default values
  //ratios
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_front_left", &thrust_ratios[H_F_L]));
  RCCHECK(rclc_parameter_get_double(&param_server,"thrust_ratio_front_right" , &thrust_ratios[H_F_R]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_back_left", &thrust_ratios[H_B_L]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_back_right", &thrust_ratios[H_B_R]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_v_front_left", &thrust_ratios[V_F_L]));
  RCCHECK(rclc_parameter_get_double(&param_server,"thrust_ratio_v_front_right" , &thrust_ratios[V_F_R]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_v_back_left", &thrust_ratios[V_B_L]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_v_back_right", &thrust_ratios[V_B_R]));

  //directions
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_front_left", &thrust_directions[H_F_L]));
  RCCHECK(rclc_parameter_get_int(&param_server,"thrust_direction_front_right" , &thrust_directions[H_F_R]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_back_left", &thrust_directions[H_B_L]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_back_right", &thrust_directions[H_B_R]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_v_front_left", &thrust_directions[V_F_L]));
  RCCHECK(rclc_parameter_get_int(&param_server,"thrust_direction_v_front_right" , &thrust_directions[V_F_R]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_v_back_left", &thrust_directions[V_B_L]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_v_back_right", &thrust_directions[V_B_R]));

}

/*bool on_parameter_changed(const Parameter *old_param, const Parameter *new_param, void *context)
{
    (void) context;

    if (old_param == NULL && new_param == NULL)
    {
        
        return false;
    }

    if (new_param == NULL)
    {
        
        return false;
    }

    // Handle thrust ratio parameters using strcmp
    if (strcmp(new_param->name.data, "thrust_ratio_front_left") == 0) {
        thrust_ratios[H_F_L] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_front_right") == 0) {
        thrust_ratios[H_F_R] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_back_left") == 0) {
        thrust_ratios[H_B_L] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_back_right") == 0) {
        thrust_ratios[H_B_R] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_v_front_left") == 0) {
        thrust_ratios[V_F_L] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_v_front_right") == 0) {
        thrust_ratios[V_F_R] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_v_back_left") == 0) {
        thrust_ratios[V_B_L] = new_param->value.double_value;
    } else if (strcmp(new_param->name.data, "thrust_ratio_v_back_right") == 0) {
        thrust_ratios[V_B_R] = new_param->value.double_value;
    }

    // Handle thrust direction parameters
    else if (strcmp(new_param->name.data, "thrust_direction_front_left") == 0) {
        thrust_directions[H_F_L] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_front_right") == 0) {
        thrust_directions[H_F_R] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_back_left") == 0) {
        thrust_directions[H_B_L] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_back_right") == 0) {
        thrust_directions[H_B_R] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_v_front_left") == 0) {
        thrust_directions[V_F_L] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_v_front_right") == 0) {
        thrust_directions[V_F_R] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_v_back_left") == 0) {
        thrust_directions[V_B_L] = new_param->value.integer_value;
    } else if (strcmp(new_param->name.data, "thrust_direction_v_back_right") == 0) {
        thrust_directions[V_B_R] = new_param->value.integer_value;
    }

    // Print debug message to confirm the change
    //printf("Parameter %s changed to %f or %d\n", new_param->name.data, new_param->value.double_value, new_param->value.integer_value);
    return true;
}*/


// Function to apply the calculated PWM values to the thrusters
void apply_pwm() {
    H_FRONT_LEFT.writeMicroseconds(pwm_values[H_F_L]);
    H_FRONT_RIGHT.writeMicroseconds(pwm_values[H_F_R]);
    H_BACK_LEFT.writeMicroseconds(pwm_values[H_B_L]);
    H_BACK_RIGHT.writeMicroseconds(pwm_values[H_B_R]);
    V_FRONT_LEFT.writeMicroseconds(pwm_values[V_F_L]);
    V_FRONT_RIGHT.writeMicroseconds(pwm_values[V_F_R]);
    V_BACK_LEFT.writeMicroseconds(pwm_values[V_B_L]);
    V_BACK_RIGHT.writeMicroseconds(pwm_values[V_B_R]);
}

// Speed message callback to control the speed multiplier
void speed_callback(const void *msg_in) {
    const std_msgs__msg__Int64 *msg = (const std_msgs__msg__Int64 *)msg_in;
    speed_multiplier = msg->data;  // Update the speed multiplier
}

// Twist message callback to control thrusters
void thruster_twist_callback(const void *msg_in) {
    const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *)msg_in;

    // Linear movement control
    float forward_back = msg->linear.x;  // Forward/backward movement
    float left_right = msg->linear.y;    // Left/right movement
    float up_down = msg->linear.z;       // Vertical movement (depth control)

    // Angular movement control
    float roll = msg->angular.x;         // Roll (rotation around x-axis)
    float pitch = msg->angular.y;        // Pitch (rotation around y-axis)
    float yaw = msg->angular.z;          // Yaw (rotation around z-axis)

    // Adjust the horizontal thrusters for forward/backward, left/right, and yaw
    pwm_values[H_F_L] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[H_F_L] * thrust_ratios[H_F_L] *
                            (forward_back * PWM_INCREMENT * speed_multiplier + left_right * PWM_INCREMENT * speed_multiplier + yaw * PWM_INCREMENT * speed_multiplier)));  // H_FRONT_LEFT
    pwm_values[H_F_R] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[H_F_R] * thrust_ratios[H_F_R] *
                            (forward_back * PWM_INCREMENT * speed_multiplier + -left_right * PWM_INCREMENT * speed_multiplier + -yaw * PWM_INCREMENT * speed_multiplier))); // H_FRONT_RIGHT
    pwm_values[H_B_L] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[H_B_L] * thrust_ratios[H_B_L] *
                            (forward_back * PWM_INCREMENT * speed_multiplier + -left_right * PWM_INCREMENT * speed_multiplier + yaw * PWM_INCREMENT * speed_multiplier))); // H_BACK_LEFT
    pwm_values[H_B_R] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[H_B_R] * thrust_ratios[H_B_R] *
                            (forward_back * PWM_INCREMENT * speed_multiplier + left_right * PWM_INCREMENT * speed_multiplier + -yaw * PWM_INCREMENT * speed_multiplier))); // H_BACK_RIGHT

    // Adjust the vertical thrusters for up/down, pitch, and roll
    pwm_values[V_F_L] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[V_F_L] * thrust_ratios[V_F_L] *
                            (up_down * PWM_INCREMENT * speed_multiplier + -pitch * PWM_INCREMENT * speed_multiplier + roll * PWM_INCREMENT * speed_multiplier)));  // V_FRONT_LEFT
    pwm_values[V_F_R] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[V_F_R] * thrust_ratios[V_F_R] *
                            (up_down * PWM_INCREMENT * speed_multiplier + -pitch * PWM_INCREMENT * speed_multiplier + -roll * PWM_INCREMENT * speed_multiplier)));  // V_FRONT_RIGHT
    pwm_values[V_B_L] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[V_B_L] * thrust_ratios[V_B_L] *
                            (up_down * PWM_INCREMENT * speed_multiplier + pitch * PWM_INCREMENT * speed_multiplier + roll * PWM_INCREMENT * speed_multiplier)));   // V_BACK_LEFT
    pwm_values[V_B_R] = clamp_pwm(NEUTRAL_PWM + (int)(thrust_directions[V_B_R] * thrust_ratios[V_B_R] *
                            (up_down * PWM_INCREMENT * speed_multiplier + pitch * PWM_INCREMENT * speed_multiplier + -roll * PWM_INCREMENT * speed_multiplier)));   // V_BACK_RIGHT

    // Apply the calculated PWM values to the thrusters
    apply_pwm();
}

// Timer callback to publish thrusters feedback
void feedback_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    (void) timer;
    (void) last_call_time;
    
    // Copy PWM values to the message
    for (int i = 0; i < 8; i++) {
        thrusters_feedback_msg.data.data[i] = pwm_values[i];
    }

    // Publish the message
    RCSOFTCHECK(rcl_publish(&thrusters_feedback_publisher, &thrusters_feedback_msg, NULL));
}

// Timer callback to check for parameters new values feedback
void parameter_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    (void) timer;
    (void) last_call_time;
    
  // Get paramter values
  //ratios
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_front_left", &thrust_ratios[H_F_L]));
  RCCHECK(rclc_parameter_get_double(&param_server,"thrust_ratio_front_right" , &thrust_ratios[H_F_R]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_back_left", &thrust_ratios[H_B_L]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_back_right", &thrust_ratios[H_B_R]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_v_front_left", &thrust_ratios[V_F_L]));
  RCCHECK(rclc_parameter_get_double(&param_server,"thrust_ratio_v_front_right" , &thrust_ratios[V_F_R]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_v_back_left", &thrust_ratios[V_B_L]));
  RCCHECK(rclc_parameter_get_double(&param_server, "thrust_ratio_v_back_right", &thrust_ratios[V_B_R]));

  //directions
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_front_left", &thrust_directions[H_F_L]));
  RCCHECK(rclc_parameter_get_int(&param_server,"thrust_direction_front_right" , &thrust_directions[H_F_R]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_back_left", &thrust_directions[H_B_L]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_back_right", &thrust_directions[H_B_R]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_v_front_left", &thrust_directions[V_F_L]));
  RCCHECK(rclc_parameter_get_int(&param_server,"thrust_direction_v_front_right" , &thrust_directions[V_F_R]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_v_back_left", &thrust_directions[V_B_L]));
  RCCHECK(rclc_parameter_get_int(&param_server, "thrust_direction_v_back_right", &thrust_directions[V_B_R]));

  // Set parameter default values
  //ratios
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_front_left", thrust_ratios[H_F_L]));
  RCCHECK(rclc_parameter_set_double(&param_server,"thrust_ratio_front_right" , thrust_ratios[H_F_R]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_back_left", thrust_ratios[H_B_L]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_back_right", thrust_ratios[H_B_R]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_v_front_left", thrust_ratios[V_F_L]));
  RCCHECK(rclc_parameter_set_double(&param_server,"thrust_ratio_v_front_right" , thrust_ratios[V_F_R]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_v_back_left", thrust_ratios[V_B_L]));
  RCCHECK(rclc_parameter_set_double(&param_server, "thrust_ratio_v_back_right", thrust_ratios[V_B_R]));

  //directions
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_front_left", thrust_directions[H_F_L]));
  RCCHECK(rclc_parameter_set_int(&param_server,"thrust_direction_front_right" , thrust_directions[H_F_R]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_back_left", thrust_directions[H_B_L]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_back_right", thrust_directions[H_B_R]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_v_front_left", thrust_directions[V_F_L]));
  RCCHECK(rclc_parameter_set_int(&param_server,"thrust_direction_v_front_right" , thrust_directions[V_F_R]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_v_back_left", thrust_directions[V_B_L]));
  RCCHECK(rclc_parameter_set_int(&param_server, "thrust_direction_v_back_right", thrust_directions[V_B_R]));

}

void setup() {
    set_microros_transports();
    // Initialize thrusters
    setup_thrusters();
    
    delay(2000);
    // Initialize micro-ROS and node
    allocator = rcl_get_default_allocator();
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
    RCCHECK(rclc_node_init_default(&node, "system_esp", "", &support));


    // Initialize thrusters feedback publisher
    RCCHECK(rclc_publisher_init_default(
        &thrusters_feedback_publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
        "thrusters_feedback"
    ));

    // Initialize the parameter server
    RCCHECK(rclc_parameter_server_init_with_option(
        &param_server,
        &node,
        &param_options
    ));
    setup_parameters();

    // Initialize the feedback message
    thrusters_feedback_msg.data.data = thrusters_feedback_data;
    thrusters_feedback_msg.data.size = 8;
    thrusters_feedback_msg.data.capacity = 8;

    // Create subscriber for Twist messages
    RCCHECK(rclc_subscription_init_default(
        &twist_subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
        "movement_twist"
    ));

    // Create subscriber for speed messages
    RCCHECK(rclc_subscription_init_default(
        &speed_subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64),
        "speed"
    ));

    // Initialize the timer for thrusters feedback
    const unsigned int feedback_timer_timeout = 100; // milliseconds
    RCCHECK(rclc_timer_init_default(&feedback_timer, &support, RCL_MS_TO_NS(feedback_timer_timeout), feedback_timer_callback));
    RCCHECK(rclc_timer_init_default(&feedback_timer, &support, RCL_MS_TO_NS(10000), parameter_timer_callback));

    // Create an executor to handle the callbacks
    RCCHECK(rclc_executor_init(&executor, &support.context, RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES + 20, &allocator));  
    RCCHECK(rclc_executor_add_subscription(&executor, &twist_subscriber, &twist_msg, &thruster_twist_callback, ON_NEW_DATA));
    RCCHECK(rclc_executor_add_subscription(&executor, &speed_subscriber, &speed_msg, &speed_callback, ON_NEW_DATA));
    RCCHECK(rclc_executor_add_timer(&executor, &feedback_timer));
    RCCHECK(rclc_executor_add_timer(&executor, &feedback_timer));
    RCCHECK(rclc_executor_add_parameter_server(&executor, &param_server, NULL));
}

void loop() {
    // Continuously spin to handle messages and control thrusters
    RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
    delay(100); // Small delay to avoid overloading the CPU
}

[hippo5329]

1. You will need to allocate memory for the multiarray message.

https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/

2. You will need to increase the allocation of publishers, subscribers, and services with user colcon meta. Parameters server needs 5 services. The default is too low.

https://micro.ros.org/docs/tutorials/advanced/microxrcedds_rmw_configuration/

You will need to rebuild library for arduino. This is much easier with platformio.

3. You may find the parameter server example in the micro-ros-demos project.

[hippo5329]

https://github.com/micro-ROS/micro-ROS-demos/blob/jazzy/rclc/parameter_server/main.c

It is difficult to debug a large program. Especially when you are new. There are a lot of things to learn. You should build and test in small steps. Verify every basic function. Add new features one by one.

[hippo5329]

You should get started with a small simple robot before you build your dream robot.

https://github.com/hippo5329/linorobot2_hardware/wiki#linorobot2_hardware-with-esp32-and-pico-support