MicroROS issues with creating publisher

[Badoniak]

Hi everyone, I have been working on a project using microROS on stm32 f446re with freertos for some time. I have a problem with creating new publishers and subscribers. Subscriber /cmd_vel works great but when I try to add a publisher or publisher and its subscriber like speed_publisher or speed_subscriber, they don't show up in the Topic List. Has anyone ever had a similar problem or know how to solve this problem? I'm using ros humble on ubuntu 22.04

In this code:

#include "tim.h"
#include "gpio.h"
#include <stdio.h>
#include <math.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>
#include <DriveControl.h>
#include <std_msgs/msg/int32.h>

// Macro functions
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); vTaskDelete(NULL);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

// Constants
#define FRAME_TIME 1 // 1000 / FRAME_TIME = FPS
#define PWM_MOTOR_MIN 0    
#define PWM_MOTOR_MAX 1000   // 

// Global variables
rcl_publisher_t publisher;
rcl_subscription_t speed_subscriber;
std_msgs__msg__Int32 speed_send_msg;
std_msgs__msg__Int32 speed_recv_msg;

geometry_msgs__msg__Twist msg;
rcl_subscription_t subscriber;
rcl_node_t node;
rclc_support_t support;
rcl_timer_t timer;
rcl_timer_t speed_timer;



// Function prototypes
void setupPins(void);
void setupRos(void);
void cmd_vel_callback(const void *msgin);
void timer_callback(rcl_timer_t *timer, int64_t last_call_time);
float fmap(float val, float in_min, float in_max, float out_min, float out_max);
void cleanup(void);

// HAL handles
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim1;

void setupPins(void) {
  
    // PWM Timer initialization -
    if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1) != HAL_OK) {
        Error_Handler();
    }
    if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2) != HAL_OK) {
        Error_Handler();
    }
    if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3) != HAL_OK) {
        Error_Handler();
    }
    if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4) != HAL_OK) {
        Error_Handler();
    }
}
void speed_subscription_callback(const void * msgin)
{
    const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
    printf("Received: %d\n", msg->data);
}
void speed_timer_callback(rcl_timer_t * speed_timer, int64_t last_call_time)
{
    (void) last_call_time;
    if (speed_timer != NULL) {
        RCSOFTCHECK(rcl_publish(&publisher, &speed_send_msg, NULL));
        printf("Sent: %d\n", speed_send_msg.data);
        speed_send_msg.data++;
    }
}
void cmd_vel_callback(const void *msgin) {
    const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msgin;
    if (twist_msg != NULL) {
        msg = *twist_msg;
    }
}

void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
    (void)last_call_time;  // Unused parameter
    if (timer == NULL) return;

    float linear = constrain(msg.linear.x, -1, 1);
    float angular = constrain(msg.angular.z, -1, 1);

    // motors calculation
    float left = linear - angular;
    float right = linear + angular;

    // Normalization
    float max_value = fmax(fabs(left), fabs(right));
    if (max_value > 1.0f) {
       left /= max_value;
       right /= max_value;
    }
    
    uint16_t pwmValueLeft = (uint16_t)fmap(fabs(left), 0, 1, PWM_MOTOR_MIN, PWM_MOTOR_MAX); 
    uint16_t pwmValueRight = (uint16_t)fmap(fabs(right), 0, 1, PWM_MOTOR_MIN, PWM_MOTOR_MAX);  
    
    if (left>0) {
        HAL_GPIO_WritePin(Direction_Left_Front_GPIO_Port, Direction_Left_Front_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(Direction_Left_Rear_GPIO_Port, Direction_Left_Rear_Pin, GPIO_PIN_RESET);
    } 
    else {
        HAL_GPIO_WritePin(Direction_Left_Front_GPIO_Port, Direction_Left_Front_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(Direction_Left_Rear_GPIO_Port, Direction_Left_Rear_Pin, GPIO_PIN_SET);
    }  
    
    if (right>0) {
        HAL_GPIO_WritePin(Direction_Right_Front_GPIO_Port, Direction_Right_Front_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(Direction_Right_Rear_GPIO_Port, Direction_Right_Rear_Pin, GPIO_PIN_SET);
    } 
    else {
        HAL_GPIO_WritePin(Direction_Right_Front_GPIO_Port, Direction_Right_Front_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(Direction_Right_Rear_GPIO_Port, Direction_Right_Rear_Pin, GPIO_PIN_RESET);
    } 
     __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwmValueLeft);
     __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, pwmValueLeft);
     
     __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, pwmValueRight);
     __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, pwmValueRight);

}

float fmap(float val, float in_min, float in_max, float out_min, float out_max) {
    if (in_max - in_min == 0) return out_min;
    return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void cleanup(void) {
    
    rcl_publisher_fini(&publisher, &node);
    rcl_timer_fini(&timer);
    rcl_timer_fini(&speed_timer);
    rcl_subscription_fini(&subscriber, &node);
    rcl_subscription_fini(&speed_subscriber, &node);
    rcl_node_fini(&node);
    rclc_support_fini(&support);
}

void appMain(void *argument) {
    (void)argument;  // Unused parameter

    // Basic inicjalizzation
    HAL_Init();
    SystemClock_Config();
    MX_GPIO_Init();
    MX_TIM2_Init();
    setupPins();

    // ROS 2 setup
    rcl_allocator_t allocator = rcl_get_default_allocator();


    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
    RCCHECK(rclc_node_init_default(&node, "ros_stm32_diffdrive", "", &support));
    
    RCCHECK(rclc_subscription_init_default(
        &subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
        "/cmd_vel"));
        // create publisher

    RCCHECK(rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "/speed_publisher"));

        // create subscriber
    RCCHECK(rclc_subscription_init_default(
        &speed_subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "/speed_subscriber"));

    RCCHECK(rclc_timer_init_default(
            &speed_timer,
            &support,
            RCL_MS_TO_NS(50),
            speed_timer_callback));
    

    RCCHECK(rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(FRAME_TIME),
        timer_callback));

    rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
    RCCHECK(rclc_executor_init(&executor, &support.context, 4, &allocator));
    RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &cmd_vel_callback, ON_NEW_DATA));
    RCCHECK(rclc_executor_add_timer(&executor, &speed_timer));
    RCCHECK(rclc_executor_add_subscription(&executor, &speed_subscriber, &speed_recv_msg, &speed_subscription_callback, ON_NEW_DATA));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));
    speed_send_msg.data = 0;

    while (1) {
        RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(FRAME_TIME)));
        HAL_Delay(FRAME_TIME);
    }

    cleanup();
}