search

micro-ROS / micro_ros_stm32cubemx_utils

A set of utilities for integrating micro-ROS in a STM32CubeMX project
Apache License 2.0
151 stars 59 forks source link

rclc_support_init() Function doesnt work and give me an RCL_RET_ERROR back #116

Open 28marcel28 opened 11 months ago

28marcel28 commented 11 months ago

Hello Microros or ROS2 Community,

Im using Ubuntu 22.04 LTS on an Raspberry Pi 4.0 (Compute Module). Im using a STM32F767ZI with the STM32CubeIDE Version 1.7 with microros stack (micro_ros_stm32cubemx_utils). I can compile and flash the MCU with docker. It still works.

The problem is, that the rclc_support_init() function doesn`t initialize rcl and doesn´t create some support data structures. Cause They give me a RCL_RET_ERROR back.

Thats the reason i cant create my Publisher.

And so, the communication between both devices doesn´t work with the Agent and the XRCE-DDS-Client on stm32.

In the github Issues I found 3 times the same problem, but they have no solution.

Issue 1: https://github.com/micro-ROS/micro_ro...

Issue 2: https://github.com/micro-ROS/micro_ro...

Issue 3: https://github.com/micro-ROS/micro_ro...

Can anyone help me with this problem? Or get anyone a solution for this problem?

Thank you for your help,

Your Microros Developer Marcel.

pablogs9 commented 11 months ago

Hello, which transport are you using?

28marcel28 commented 11 months ago

I am utilizing USART3 in combination with DMA Transport. The configuration in my .ioc file is as follows: DMA Rx and Tx are both set to a very high priority. Rx is configured to operate in circular mode. Additionally, I have enabled the global interrupt for USART3 in the NVIC settings.

For the implementation of the MicroROS Stack in STM32CubeIDE, I am referring to this video: https://www.youtube.com/watch?v=bn-P3fxtTF4

However, I am encountering an issue with the rclc_support_init() function, as it fails to generate and triggers SOMETIMES the hardfault handler. If The Hardfault handler is not triggered the RCL_RET_ERROR will me send back. I did review your previously closed issue regarding a similar problem, but I do not seem to be experiencing the same issues that were identified in that case.

pablogs9 commented 11 months ago

Could you check if the MCU is sending bytes over the serial port? Do not connect the agent for testing.

28marcel28 commented 11 months ago

Can you help me, i dont know at the moment how to force to send or to see bytes over the DMA Transport system. Im a beginner to work with the MicroROS Stack.

In Case you need more details here is my main.c file: `#if 0 / USER CODE END Header / / Includes ------------------------------------------------------------------/

include "main.h"

include "cmsis_os.h"

include "lwip.h"

/ Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes /

else

/ No FreeRTOS Wrapper Functions /

include "main.h"

include "lwip.h"

include "FreeRTOS.h"

include "task.h"

include "queue.h"

include "timers.h"

endif

/ Micro-ROS Libraries /

include <rcl/rcl.h>

include <rcl/error_handling.h>

include <rclc/rclc.h>

include <rclc/executor.h>

include <uxr/client/transport.h>

include <rmw_microxrcedds_c/config.h>

include <rmw_microros/rmw_microros.h>

/ ROS Message Typen /

include <std_msgs/msg/int32.h>

include <geometry_msgs/msg/twist.h>

/ Standard Libraries /

include "stdio.h"

include "stdlib.h"

include "string.h"

/ Ethernet TCP Socket Librearies /

include "sys/socket.h"

include "sys/types.h"

/ USER CODE END Includes /

/ Private typedef -----------------------------------------------------------/ / USER CODE BEGIN PTD /

/ Define Variables /

define HEARTBEAT_HZ 500 //Heartbeat in [ms] (1Hz)

define SOCK_PORT 8888 // Server Socket Port

define SOCK_ADDR "192.168.178.38" // Server Socket Adress

/ Subscription /

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)){error_loop();}}

/ USER CODE END PTD /

/ Private define ------------------------------------------------------------/ / USER CODE BEGIN PD /

/ USER CODE END PD /

/ Private macro -------------------------------------------------------------/ / USER CODE BEGIN PM /

if 0

/ USER CODE END PM /

/ Private variables ---------------------------------------------------------/

UART_HandleTypeDef huart3; DMA_HandleTypeDef hdma_usart3_rx; DMA_HandleTypeDef hdma_usart3_tx;

/ Definitions for defaultTask / osThreadId_t defaultTaskHandle; const osThreadAttr_t defaultTask_attributes = { .name = "defaultTask", .stack_size = 3500 4, .priority = (osPriority_t) osPriorityNormal, }; / USER CODE BEGIN PV */

else

/ Private variables ---------------------------------------------------------/

UART_HandleTypeDef huart3; DMA_HandleTypeDef hdma_usart3_rx; DMA_HandleTypeDef hdma_usart3_tx;

/ Software Timer Handler / TimerHandle_t xHeartbeat;

endif

if 0

/ USER CODE END PV /

/ Private function prototypes -----------------------------------------------/ void SystemClock_Config(void); static void MX_GPIO_Init(void); static void MX_DMA_Init(void); static void MX_USART3_UART_Init(void); void StartDefaultTask(void *argument);

/ USER CODE BEGIN PFP /

else

/ Private function prototypes -----------------------------------------------/ void SystemClock_Config(void); static void MX_GPIO_Init(void); static void MX_DMA_Init(void); static void MX_USART3_UART_Init(void);

endif

/ Tasks created / void vTask_Publisher(void pvParameters); void vTask_Subscriber(void pvParameters);

/ Functions needed for Publishing / bool cubemx_transport_open(struct uxrCustomTransport transport); bool cubemx_transport_close(struct uxrCustomTransport transport); size_t cubemx_transport_write(struct uxrCustomTransport transport, const uint8_t buf, size_t len, uint8_t err); size_t cubemx_transport_read(struct uxrCustomTransport transport, uint8_t buf, size_t len, int timeout, uint8_t err);

void microros_allocate(size_t size, void state); void microros_deallocate(void pointer, void state); void microros_reallocate(void pointer, size_t size, void state); void microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void * state);

/ USER CODE END PFP /

/ Private user code ---------------------------------------------------------/ / USER CODE BEGIN 0 /

/ Heartbeat Callback / void vHeartbeatCallback(TimerHandle_t xTimer) { / Toggle Blue LED with 1 Hz / HAL_GPIO_TogglePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin); }

/ Twist Message Callback for Subscription / void subscription_callback(const void msgin) { const geometry_msgsmsgTwist msg = (const geometry_msgsmsgTwist *)msgin;

/**** If Velocity in x direction is 0 toggle LED ****/
if(msg->linear.x ==0){

    HAL_GPIO_TogglePin(LED_RED_GPIO_Port, LED_RED_Pin);
}

}

void error_loop(){} / USER CODE END 0 /

/**

if 0

/* USER CODE END 2 */

/* Init scheduler */
osKernelInitialize();

/* USER CODE BEGIN RTOS_MUTEX */

else

endif

/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */

/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */

/* USER CODE BEGIN RTOS_TIMERS */

/**** Create Software Timer  ****/
xHeartbeat= xTimerCreate ("Heartbeat", pdMS_TO_TICKS(HEARTBEAT_HZ), pdTRUE, 0, vHeartbeatCallback);

/**** Timer Start ****/
xTimerStart(xHeartbeat,0);

/* USER CODE END RTOS_TIMERS */

/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */

if 0

/* USER CODE END RTOS_QUEUES */

/* Create the thread(s) */
/* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

/* USER CODE BEGIN RTOS_THREADS */

else

if(xTaskCreate(vTask_Publisher, "Publisher", 3500, NULL, 3, NULL) != pdPASS)
{
    /**** Task not created ****/
    Error_Handler();
}

//  if(xTaskCreate(vTask_Subscriber, "Subscriber", 3000, NULL, 3, NULL) != pdPASS)
//  {
//      /**** Task not created ****/
//      Error_Handler();
//  }

endif

/* add threads, ... */
/* USER CODE END RTOS_THREADS */

/* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */

if 0

/* USER CODE END RTOS_EVENTS */

/* Start scheduler */
osKernelStart();

/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */

else

/**** Start Scheduler ****/
vTaskStartScheduler();

endif

while (1)
{
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */

}

/**

/**

}

/**

}

/**

}

/ USER CODE BEGIN 4 /

/ USER CODE END 4 /

/ USER CODE BEGIN 5 /

void vTask_Publisher(void *pvParameters) { /***** micro-ROS Initialization configuration ****/

if 0

struct sockaddr_in remote_addr;
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(SOCK_PORT);
remote_addr.sin_addr.s_addr = inet_addr(SOCK_ADDR);

/**** Sets micro-ROS default custom transport ****/
rmw_uros_set_custom_transport(
        true, // Enable XRCE framing
        (void *) &remote_addr,
        cubemx_transport_open,
        cubemx_transport_close,
        cubemx_transport_write,
        cubemx_transport_read);

else

    /**** Sets micro-ROS default custom transport ****/
    rmw_uros_set_custom_transport(
            true, // Enable XRCE framing
            (void *) &huart3,
            cubemx_transport_open,
            cubemx_transport_close,
            cubemx_transport_write,
            cubemx_transport_read);

endif

rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
freeRTOS_allocator.allocate = microros_allocate;
freeRTOS_allocator.deallocate = microros_deallocate;
freeRTOS_allocator.reallocate = microros_reallocate;
freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

if (!rcutils_set_default_allocator(&freeRTOS_allocator))
{
    printf("Error on default allocators (line %d)\n", __LINE__);
}

/************************************************ micro-ROS app *******************************************/

/**** creating Publisher, msg, allocator, support und Node ****/
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator= rcl_get_default_allocator();

if 1

/**** Initialize and modify options (Set ROS Domain ID) ****/
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
rcl_init_options_init(&init_options, allocator); // IF 0 = RCL_RET_OK;
//rcl_init_options_fini(&init_options);
rcl_init_options_set_domain_id(&init_options, 11); // Domain ID is 11 | IF 0 = RCL_RET_OK;

/**** create init_options ****/
uint8_t test_supp_init=0;
if ((test_supp_init= rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator)) == (RCL_RET_INVALID_ARGUMENT || RCL_RET_ERROR))
{
    /**** Cant Create init options ****/
    //Error_Handler();
}

else

/**** create init_options ****/
uint8_t test_supp_init=0;
if ((test_supp_init= rclc_support_init(&support, 0, NULL, &allocator)) == (RCL_RET_INVALID_ARGUMENT || RCL_RET_ERROR))
{
    /**** Cant Create init options ****/
    //Error_Handler();
}

endif

/**** create node ****/
rclc_node_init_default(&node, "publisher_node", "" , &support);

/**** Create Message, topic and publisher ****/
rcl_publisher_t publisher;
const char * topic = "kurwa";
const rosidl_message_type_support_t * msg_type = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

/**** create publisher ****/
if(rclc_publisher_init_default(&publisher, &node, msg_type, topic) != RCL_RET_OK)
{
    /**** Cant Create Publisher ****/
    //Error_Handler();
}

/**** published message has to be allocated statically ****/
std_msgs__msg__Int32 pub_msg;
std_msgs__msg__Int32__init(&pub_msg);

/**** Set Counter ****/
pub_msg.data=0;

/**** Ueberpruefe ob publisher funktioniert ****/

//RCL_RET_INVALID_ARGUMENT; value=11
//RCL_RET_PUBLISHER_INVALID; value=300
//RCL_RET_ERROR; value=1
//RCL_RET_BAD_ALLOC; value 10
//RCL_RET_OK; value=0

for(;;)
{
    /**** Publish the Message ****/
    rcl_ret_t ret = rcl_publish(&publisher, &pub_msg, NULL);
    if (ret != RCL_RET_OK)
    {
        /**** Error Publishing ****/
        printf("Error publishing (line %d)\n", __LINE__);
    }

    /**** Increment Counter ****/
    pub_msg.data++;
    vTaskDelay(10);

} // end for(;;)

/******************************************* Clean Up ********************************************************/

/****Destroy created entities ****/
rcl_publisher_fini(&publisher, &node);

/**** Destroy node ****/
rcl_node_fini(&node);

/**** Dealocates Object ****/
rclc_support_fini(&support);

} // end Publisher Task

void vTask_Subscriber(void *pvParameters) { / micro-ROS Initialisation configuration /

/**** Sets micro-ROS default custom transport ****/
rmw_uros_set_custom_transport(
        true, // Set XCRE-Agent on
        (void *) &huart3,
        cubemx_transport_open,
        cubemx_transport_close,
        cubemx_transport_write,
        cubemx_transport_read);

rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
freeRTOS_allocator.allocate = microros_allocate;
freeRTOS_allocator.deallocate = microros_deallocate;
freeRTOS_allocator.reallocate = microros_reallocate;
freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

if (!rcutils_set_default_allocator(&freeRTOS_allocator))
{
    printf("Error on default allocators (line %d)\n", __LINE__);
}

/**** Micro Ros App ****/
rcl_subscription_t subscriber;
geometry_msgs__msg__Twist msg;
rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;

allocator = rcl_get_default_allocator();

/**** create init options ****/
RCCHECK(rclc_support_init(&support,0,NULL, &allocator));

/**** create node ****/
RCCHECK(rclc_node_init_default(&node, "subscriber_node", "", &support));

/**** create subscriber ****/
RCCHECK(rclc_subscription_init_default(&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "kurwa_subscriber"));

/**** create executor ****/
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));

for(;;)
{
    vTaskDelay(1000);
    RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

}

/ USER CODE END 5 /

/**

/**

ifdef USE_FULL_ASSERT

/**

pablogs9 commented 11 months ago

I just remembered this issue where there were problems in rclc_support_init: https://github.com/micro-ROS/micro_ros_stm32cubemx_utils/issues/110

It was a matter of allocators, could you just read this issue and replicate its solution?

28marcel28 commented 11 months ago

So, i understand, that the problem can be a bad Allocation. Does that mean, i can use the normal alloc malloc, free functions to replace following: rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator(); freeRTOS_allocator.allocate = microros_allocate; freeRTOS_allocator.deallocate = microros_deallocate; freeRTOS_allocator.reallocate = microros_reallocate; freeRTOS_allocator.zero_allocate = microros_zero_allocate;

I couldnt see any wrapper functions to solve the problem in the mentioned issue: #110

danielallstar commented 10 months ago

Hi @28marcel28, did you find a solution for this problem?

28marcel28 commented 10 months ago

Hey @danielallstar, we didnt find a solution for the STM32F767ZI. But we changed the board to the STM32F401re and it works fine! But we assume that the Transport Layer has an Hardware problem via USART and UART. This board isnt compatible for the present Microros-Stack!

zp1915 commented 1 month ago

@danielallstar @28marcel28 I have the same problem as you using the STM32F103ZE. finally,I find a solution from the suggetion of pablogs9. the stack of microros task is too small. const osThreadAttr_t defaultTask_attributes = { .name = "defaultTask", .stack_size = 128 16, //.stack_size = 128 4 .priority = (osPriority_t) osPriorityNormal, };