microRos publisher and services

[duncanjovial]

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