MicroRos subscriber callback stops getting called after receiving a couple messages #138
Comments
|
Could you share the whole code? |
/*
* cmr_micro_ros.c
*
* Created on: Feb 25, 2023
* Author: ajsco
*/
#include "cmr_micro_ros.h"
#include "main.h"
#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>
#include <micro_ros_utilities/type_utilities.h>
#include <std_msgs/msg/int32.h>
#include "cmr_msgs/msg/motor_write_batch.h"
#include "cmr_msgs/msg/sensor_read_batch.h"
#include "cmr_msgs/msg/power.h"
#include "cmr_msgs/msg/error.h"
#include "cmr_msgs/msg/heartbeat.h"
#include "boards.h"
//global variables
#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);}}
extern UART_HandleTypeDef huart1;
extern TIM_HandleTypeDef htim6;
extern uint8_t MROS_REINIT_FLAG;
rcl_allocator_t freeRTOS_allocator;
// uROS definitions
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rclc_executor_t executor;
// error publisher definitions
rcl_publisher_t error_pub;
cmr_msgs__msg__Error error_pub_msg;
// sensor publisher definitions
rcl_publisher_t sensor_pub;
cmr_msgs__msg__SensorReadBatch sensor_pub_msg;
uint8_t sensor_ids[] = { BLDC_D1, BLDC_D2, BLDC_D3, BLDC_D4, BLDC_D5, BLDC_D6,
BLDC_AR1, BLDC_AR2, BLDC_AR3, BLDC_AR4, BLDC_AR5, BLDC_AR6,
SENSOR1, SENSOR2 };
size_t num_sensors = sizeof(sensor_ids) / sizeof(uint8_t);
//cmr_msgs__msg__SensorReadBatch sensor_msg_buf; // buffer to transiently store enqueued sensor messages
// heartbeat subscriber definitions
rcl_subscription_t heartbeat_sub;
cmr_msgs__msg__Heartbeat heartbeat_sub_msg;
// motor subscriber definitions
rcl_subscription_t motor_sub;
cmr_msgs__msg__MotorWriteBatch motor_sub_msg;
//cmr_msgs__msg__MotorWriteBatch motor_msg_buf; // buffer to transiently store enqueued motor messages
// power subscriber definitions
rcl_subscription_t power_sub;
cmr_msgs__msg__Power power_sub_msg;
//cmr_msgs__msg__Power power_msg_buf; // buffer to transiently store enqueued power messages
static micro_ros_utilities_memory_conf_t conf = {0};
int first_time = 0;
//free-rtos external variables
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);
bool alloc_msgs(void);
bool dealloc_msgs(void);
// callbacks
int calls = 0;
void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
(void) last_call_time;
if (timer == NULL){
return;
}
//publish data back to Jetson
sensor_pub_msg.sensor_ids.data = sensor_ids;
sensor_pub_msg.sensor_ids.size = num_sensors;
sensor_pub_msg.size = num_sensors;
// sensor values are set by HAL_CAN_RxFifo0MsgPendingCallback in cmr_can.c
// just need to set the size here (26 total fields).
sensor_pub_msg.values.size = 26;
// RCSOFTCHECK(rcl_publish(&sensor_pub, &sensor_pub_msg, NULL));
// if( rcl_publish(&sensor_pub, &sensor_pub_msg, NULL) != RCL_RET_OK ){
// NVIC_SystemReset();
// }
// if(error_pub_msg.code != 0){
// RCSOFTCHECK(rcl_publish(&error_pub, &error_pub_msg, NULL));
//
// //maybe check this to see when to clear error
// error_pub_msg.code = 0;
// }
}
void heartbeat_sub_callback(const void *msgin) {
HAL_GPIO_TogglePin(GPIOC, LED_DEBUG_Pin);
// osDelay(60);
// HAL_GPIO_WritePin(GPIOC, LED_DEBUG_Pin, GPIO_PIN_RESET);
// osDelay(60);
// const cmr_msgs__msg__Heartbeat *msg = (const cmr_msgs__msg__Heartbeat*) msgin;
//
// if (msg->heartbeat > 0){
// osStatus_t os = osMessageQueuePut(heartbeatQueueHandle, msg, 0U, 0U);
// }
}
void motor_sub_callback(const void *msgin) {
HAL_GPIO_TogglePin(GPIOC, LED_DEBUG_Pin);
// const cmr_msgs__msg__MotorWriteBatch *msg = (const cmr_msgs__msg__MotorWriteBatch*) msgin;
//
//// osSemaphoreAcquire(motorSemHandle, osWaitForever);
//
// if(msg->size > 0){
// osStatus_t os = osMessageQueuePut(canMotorQueueHandle, msg, 0U, 0U);
// if (os != osOK) {
// printf("motor cannot add to queue");
// }
// }
//// //refresh watchdog only if we know information is coming in
// if(first_time != 0){
// //HAL_IWDG_Refresh(&hiwdg);
// }
// else{
// //MX_IWDG_Init();
// first_time = 1;
// }
// osSemaphoreRelease(motorSemHandle);
//todo add feature to continously try to add for 4 tries
}
void power_sub_callback(const void *msgin) {
const cmr_msgs__msg__Power *msg = (const cmr_msgs__msg__Power*) msgin;
// osSemaphoreAcquire(motorSemHandle, osWaitForever);
osStatus_t os = osMessageQueuePut(canPowQueueHandle, msg, 0U, 0U);
// osSemaphoreRelease(motorSemHandle);
//todo add feature to continously try to add for 4 tries
if (os != osOK) {
printf("power cannot add to queue");
}
}
// helper functions
bool alloc_msgs() {
conf.max_basic_type_sequence_capacity = 32;
// Configure and allocate motor subscriber message
bool success = micro_ros_utilities_create_message_memory(
ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, MotorWriteBatch),
&motor_sub_msg,
conf
);
if (!success) return false;
//
// // Configure and allocate power subscriber message
// success = micro_ros_utilities_create_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Power),
// &power_sub_msg,
// conf
// );
// if (!success) return false;
//
// // Configure and allocate sensor publisher message
// success = micro_ros_utilities_create_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, SensorReadBatch),
// &sensor_pub_msg,
// conf
// );
// if (!success) return false;
//
// // Configure and allocate error publisher message
// success = micro_ros_utilities_create_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Error),
// &error_pub_msg,
// conf
// );
// if (!success) return false;
// bool success = micro_ros_utilities_create_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Heartbeat),
// &heartbeat_sub_msg,
// conf
// );
// if (!success) return false;
return true;
}
bool dealloc_msgs() {
// Configure and allocate motor subscriber message
// bool success = micro_ros_utilities_destroy_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, MotorWriteBatch),
// &motor_sub_msg,
// conf
// );
// if (!success) return false;
//
// // Configure and allocate power subscriber message
// success = micro_ros_utilities_destroy_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Power),
// &power_sub_msg,
// conf
// );
// if (!success) return false;
//
// // Configure and allocate sensor publisher message
// success = micro_ros_utilities_destroy_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, SensorReadBatch),
// &sensor_pub_msg,
// conf
// );
// if (!success) return false;
//
// // Configure and allocate error publisher message
// success = micro_ros_utilities_destroy_message_memory(
// ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Error),
// &error_pub_msg,
// conf
// );
// if (!success) return false;
// Destroy the heartbeat memory
bool success = micro_ros_utilities_destroy_message_memory(
ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Heartbeat),
&heartbeat_sub_msg,
conf
);
if (!success) return false;
return true;
}
// initialization
void mros_init(void *argument) {
HAL_GPIO_TogglePin(GPIOC, LED_DEBUG_Pin);
if (rmw_uros_set_custom_transport(
true, (void*) &huart1, cubemx_transport_open, cubemx_transport_close,
cubemx_transport_write, cubemx_transport_read) != RMW_RET_OK){
NVIC_SystemReset();
}
//allocate freeRtos
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__);
NVIC_SystemReset();
}
// micro-ROS app
rcl_allocator_t allocator = rcl_get_default_allocator();
//create init_options
rcl_ret_t ret3 = rclc_support_init(&support, 0, NULL, &allocator);
if (ret3 != RCL_RET_OK) {
NVIC_SystemReset();
}
// create node
rcl_ret_t ret0 = rclc_node_init_default(&node, "ccb", "", &support);
if (ret0 != RCL_RET_OK) {
NVIC_SystemReset();
}
// create error publisher
// RCCHECK(rclc_publisher_init_default(
// &error_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Error), "/ccb/errors"));
//
// // create sensor publisher
// RCCHECK(rclc_publisher_init_best_effort(
// &sensor_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, SensorReadBatch), "/ccb/sensors"));
// heartbeat subscriber
// RCCHECK(rclc_subscription_init_best_effort(
// &heartbeat_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Heartbeat), "/ccb/heartbeat"));
// create motor subscriber
RCCHECK(rclc_subscription_init_best_effort(
&motor_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, MotorWriteBatch), "/ccb/motors"));
//
// // create power subscriber
// RCCHECK(rclc_subscription_init_default(
// &power_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(cmr_msgs, msg, Power), "/ccb/power"));
// create timer
rcl_timer_t timer;
const unsigned int timer_rate = 100; // 1 Hz (arbitrary value)
RCCHECK(
rclc_timer_init_default( &timer, &support, RCL_MS_TO_NS(timer_rate), timer_callback));
//init variables
// error_pub_msg.code = 0;
//
// //create sensor data arrays
// uint8_t sensor_ids[] = { BLDC_D1, BLDC_D2, BLDC_D3, BLDC_D4, BLDC_D5, BLDC_D6,
// BLDC_AR1, BLDC_AR2, BLDC_AR3, BLDC_AR4, BLDC_AR5, BLDC_AR6,
// SENSOR1, SENSOR2 };
//
// int32_t values[] = { 0, 0, 0, 0, 0, 0,//drives
// 0, 0, 0, 0, 0, 0, //arm
// 0, // LIDAR
// 0, 0, 0,//accel IMU X Y Z
// 0, 0, 0,//GYRO IMU X Y Z
// 0, 0, 0, //MAG IMU X Y Z
// 0, 0, 0, 0 };//CO2
//
// sensor_pub_msg.sensor_ids.data = sensor_ids;
// sensor_pub_msg.sensor_ids.size = 14;
// sensor_pub_msg.sensor_ids.capacity = 14;
//
// sensor_pub_msg.values.data = values;
// sensor_pub_msg.values.size = 26;
// sensor_pub_msg.values.capacity = 26;
// allocate message memory
bool alloc_success = alloc_msgs();
if (!alloc_success) {
NVIC_SystemReset();
}
// create executor
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
const size_t num_handles = 2; // number of subscribers & timers
RCCHECK(rclc_executor_init(&executor, &support.context, num_handles, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
RCCHECK(rclc_executor_add_subscription(&executor, &motor_sub, &motor_sub_msg,
&motor_sub_callback, ON_NEW_DATA));
//
// RCCHECK(rclc_executor_add_subscription(&executor, &power_sub, &power_sub_msg,
// &power_sub_callback, ON_NEW_DATA));
// RCCHECK(rclc_executor_add_subscription(&executor, &heartbeat_sub, &heartbeat_sub_msg,
// &heartbeat_sub_callback, ON_NEW_DATA));
HAL_GPIO_TogglePin(GPIOC, LED_DEBUG_Pin);
// HAL_TIM_Base_Start_IT(&htim6);
rclc_executor_spin(&executor);
// for (;;) {
// rclc_executor_spin_some(&executor, 1000);
//// if (MROS_REINIT_FLAG == 1){
//// HAL_TIM_Base_Stop_IT(&htim6);
//// break;
//// }
//
// size_t freeMemory = xPortGetFreeHeapSize();
//
// // Check if available memory is below threshold
// if (freeMemory < 1000) {
// // Blink LED (Assuming GPIO_PIN_LED is the pin for the LED)
// HAL_GPIO_WritePin(GPIOC, LED_UART_Pin, GPIO_PIN_SET);
// }
// osDelay(100);
// }
//ideally never gets here since executer spins forever
NVIC_SystemReset();
dealloc_msgs();
RCCHECK(rcl_subscription_fini(&motor_sub, &node));
RCCHECK(rcl_subscription_fini(&power_sub, &node));
RCCHECK(rcl_publisher_fini(&error_pub, &node));
RCCHECK(rcl_publisher_fini(&sensor_pub, &node));
RCCHECK(rcl_subscription_fini(&heartbeat_sub, &node));
RCCHECK(rcl_node_fini(&node));
RCCHECK(rclc_support_fini(&support));
// mros_init(NULL);
//NVIC_SystemReset();
}
int RCCHECK(rcl_ret_t 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);
NVIC_SystemReset();
// dealloc_msgs();
// RCCHECK(rcl_subscription_fini(&motor_sub, &node));
// RCCHECK(rcl_subscription_fini(&power_sub, &node));
// RCCHECK(rcl_publisher_fini(&error_pub, &node));
// RCCHECK(rcl_publisher_fini(&sensor_pub, &node));
// RCCHECK(rcl_subscription_fini(&heartbeat_sub, &node));
// RCCHECK(rcl_node_fini(&node));
// RCCHECK(rclc_support_fini(&support));
// mros_init(NULL);
return -1;
}
return 1;
}Here is all the code. I have commented out a lot of other publishers/subscribers because I was trying to isolate the issue to only the "heartbeat." In this case, the heartbeat is my own custom topic that I am just trying to get to work the way I expect before I move onto other things. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
I am using microRos on an STM32 device which is connected to a host agent over serial. I am attempting to do a simple function first. Basically, I have the STM32 subscribing to a "heartbeat" topic. The message type (.msg file) is simply "uint8 heartbeat" The host agent publishes a "1" on this topic.
The STM32 has a callback function which I have setup. Ideally, everytime it sees a "1", an LED will toggle.
What is currently happening though, is that the LED toggles a couple times (about 11 times) before it stops toggling, even though the host agent is still publishing. The details of my STM32 implementation are below:
Above I have shown the initialization function for micro ros. You can see that I am creating the subscriber and then attaching the callback function. The callback function is simple, shown below:
As you can see, I am just toggling an LED. The host agent publishes data on this topic every 5ms, so I expect that this callback should be invoked continuously. Instead, it gets invoked about 11 times and then stops. Does anyone have any clues as to what could be the problem?
The text was updated successfully, but these errors were encountered: