cancel
Showing results for 
Search instead for 
Did you mean: 

microRos publisher and services

duncanjovial
Associate

I want to create two nodes program, one node has two publishers and the other one handles a service

If I create each node independently both work, however, when running both simultaneously the program crashes ones the service is called.

 

 

 

void StartDefaultTask(void *argument) { rmw_uros_set_custom_transport( true, (void *) &huart2, 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; rcl_node_t node; rclc_support_t support; rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_init(&support, 0, NULL, &allocator); rclc_node_init_default(&node, "controlST", "neurobot", &support); rcl_node_t node2; //Not used in the present code but already tried //rclc_support_t support2; //rcl_allocator_t allocator2 = rcl_get_default_allocator(); //rclc_support_init(&support2, 0, NULL, &allocator2); // rclc_node_init_default(&node2, "controlST_2", "neurobot1", &support); rcl_publisher_t publisher; rcl_publisher_t publisher1; std_msgs__msg__Int32 msg; std_msgs__msg__Float32MultiArray arrayMessage; std_msgs__msg__Float32MultiArray__init(&arrayMessage); size_t capacity = 8; // Set capacity arrayMessage.data.data = (float *)malloc(capacity * sizeof(float)); // Allocate memory for data arrayMessage.data.size = capacity; // Set the size arrayMessage.data.capacity = capacity; // Set the capacity for (size_t i = 0; i < capacity; i++) { arrayMessage.data.data[i] = 0.0; // Filling with 1.0, 2.0, 3.0, 4.0, 5.0 } // create publisher rclc_publisher_init_default( &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "cubemx_publisher"); rclc_publisher_init_default( &publisher1, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "cubemx_publisher1"); msg.data = 0; rcl_service_t service; rcl_ret_t ret = rclc_service_init_default( &service, &node2, ROSIDL_GET_SRV_TYPE_SUPPORT(modify_variables,srv,ModifyVariables), "modify_variables"); const size_t num_handles = 3; // number of subscribers & timers. ¿¿Serían 5?? rclc_executor_t executor=rclc_executor_get_zero_initialized_executor(); rclc_executor_init(&executor, &support.context, num_handles, &allocator); // Para agregarle al ejecutor el suscriptor void * taskBuffer = (void *) malloc(sizeof(float) * 8); // Example: taskBuffer for 8 floats void * response = (void *) malloc(sizeof(rosidl_runtime_c__String)); // Response buffer ret = rclc_executor_add_service(&executor, &service, taskBuffer,response,service_callback ); for(;;) { rcl_publish(&publisher, &msg, NULL); rcl_publish(&publisher1, &arrayMessage, NULL); msg.data++; for (size_t i = 0; i < capacity; i++) { //arrayMessage.data.data[i]=adcValues[i]; // Filling with 1.0, 2.0, 3.0, 4.0, 5.0 arrayMessage.data.data[i]=prueba[i]; } rclc_executor_spin_some(&executor, 1* (1000 * 1000)); osDelay(10); } free(arrayMessage.data.data); /* USER CODE END 5 */ }
View more

 

 

0 REPLIES 0