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 */
}
