Open Nivant11 opened 8 months ago
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.
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?